git commit -m "first commit"

navigations/geometry2/tf2_ros/src/buffer.cpp

@@ -0,0 +1,201 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Wim Meeussen */
+
+
+#include "tf2_ros/buffer.h"
+
+#include <ros/assert.h>
+#include <sstream>
+
+namespace tf2_ros
+{
+
+static const double CAN_TRANSFORM_POLLING_SCALE = 0.01;
+
+Buffer::Buffer(ros::Duration cache_time, bool debug) :
+  BufferCore(cache_time)
+{
+  if(debug && !ros::service::exists("~tf2_frames", false))
+  {
+    ros::NodeHandle n("~");
+    frames_server_ = n.advertiseService("tf2_frames", &Buffer::getFrames, this);
+  }
+}
+
+geometry_msgs::TransformStamped 
+Buffer::lookupTransform(const std::string& target_frame, const std::string& source_frame,
+                        const ros::Time& time, const ros::Duration timeout) const
+{
+  canTransform(target_frame, source_frame, time, timeout);
+  return lookupTransform(target_frame, source_frame, time);
+}
+
+
+geometry_msgs::TransformStamped 
+Buffer::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+                        const std::string& source_frame, const ros::Time& source_time,
+                        const std::string& fixed_frame, const ros::Duration timeout) const
+{
+  canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
+  return lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame);
+}
+
+/** This is a workaround for the case that we're running inside of
+    rospy and ros::Time is not initialized inside the c++ instance. 
+    This makes the system fall back to Wall time if not initialized.  
+*/
+ros::Time now_fallback_to_wall()
+{
+  try
+  {
+    return ros::Time::now();
+  }
+  catch (ros::TimeNotInitializedException ex)
+  {
+    ros::WallTime wt = ros::WallTime::now(); 
+    return ros::Time(wt.sec, wt.nsec); 
+  }
+}
+
+/** This is a workaround for the case that we're running inside of
+    rospy and ros::Time is not initialized inside the c++ instance. 
+    This makes the system fall back to Wall time if not initialized.  
+    https://github.com/ros/geometry/issues/30
+*/
+void sleep_fallback_to_wall(const ros::Duration& d)
+{
+  try
+  {
+      d.sleep();
+  }
+  catch (ros::TimeNotInitializedException ex)
+  {
+    ros::WallDuration wd = ros::WallDuration(d.sec, d.nsec); 
+    wd.sleep();
+  }
+}
+
+void conditionally_append_timeout_info(std::string * errstr, const ros::Time& start_time,
+                                       const ros::Duration& timeout)
+{
+  if (errstr)
+  {
+    std::stringstream ss;
+    ss << ". canTransform returned after "<< (now_fallback_to_wall() - start_time).toSec() \
+       <<" timeout was " << timeout.toSec() << ".";
+    (*errstr) += ss.str();
+  }
+}
+
+bool
+Buffer::canTransform(const std::string& target_frame, const std::string& source_frame, 
+                     const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
+{
+  // Clear the errstr before populating it if it's valid.
+  if (errstr)
+    {
+      errstr->clear();
+    }
+
+  if (!checkAndErrorDedicatedThreadPresent(errstr))
+    return false;
+
+  // poll for transform if timeout is set
+  ros::Time start_time = now_fallback_to_wall();
+  const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE;
+  while (now_fallback_to_wall() < start_time + timeout && 
+         !canTransform(target_frame, source_frame, time) &&
+         (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
+         (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
+    {
+      sleep_fallback_to_wall(sleep_duration);
+    }
+  bool retval = canTransform(target_frame, source_frame, time, errstr);
+  conditionally_append_timeout_info(errstr, start_time, timeout);
+  return retval;
+}
+
+    
+bool
+Buffer::canTransform(const std::string& target_frame, const ros::Time& target_time,
+                     const std::string& source_frame, const ros::Time& source_time,
+                     const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
+{
+  // Clear the errstr before populating it if it's valid.
+  if (errstr)
+    {
+      errstr->clear();
+    }
+
+  if (!checkAndErrorDedicatedThreadPresent(errstr))
+    return false;
+
+  // poll for transform if timeout is set
+  ros::Time start_time = now_fallback_to_wall();
+  const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE;
+  while (now_fallback_to_wall() < start_time + timeout && 
+         !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) &&
+         (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) &&  //don't wait when we detect a bag loop
+         (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf)
+         {  
+           sleep_fallback_to_wall(sleep_duration);
+         }
+  bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr);
+  conditionally_append_timeout_info(errstr, start_time, timeout);
+  return retval; 
+}
+
+
+bool Buffer::getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) 
+{
+  res.frame_yaml = allFramesAsYAML();
+  return true;
+}
+
+
+
+bool Buffer::checkAndErrorDedicatedThreadPresent(std::string* error_str) const
+{
+  if (isUsingDedicatedThread())
+    return true;
+  
+
+
+  if (error_str)
+    *error_str = tf2_ros::threading_error;
+
+  ROS_ERROR("%s", tf2_ros::threading_error.c_str());
+  return false;
+}
+
+
+
+}

navigations/geometry2/tf2_ros/src/buffer_client.cpp

@@ -0,0 +1,162 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <tf2_ros/buffer_client.h>
+
+namespace tf2_ros
+{
+  BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding): 
+    client_(ns), 
+    check_frequency_(check_frequency),
+    timeout_padding_(timeout_padding)
+  {
+  }
+
+  geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame,
+      const ros::Time& time, const ros::Duration timeout) const
+  {
+    //populate the goal message
+    tf2_msgs::LookupTransformGoal goal;
+    goal.target_frame = target_frame;
+    goal.source_frame = source_frame;
+    goal.source_time = time;
+    goal.timeout = timeout;
+    goal.advanced = false;
+
+    return processGoal(goal);
+  }
+
+  geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
+      const std::string& source_frame, const ros::Time& source_time,
+      const std::string& fixed_frame, const ros::Duration timeout) const
+  {
+    //populate the goal message
+    tf2_msgs::LookupTransformGoal goal;
+    goal.target_frame = target_frame;
+    goal.source_frame = source_frame;
+    goal.source_time = source_time;
+    goal.timeout = timeout;
+    goal.target_time = target_time;
+    goal.fixed_frame = fixed_frame;
+    goal.advanced = true;
+
+    return processGoal(goal);
+  }
+
+  geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const
+  {
+    client_.sendGoal(goal);
+
+    //this shouldn't happen, but could in rare cases where the server hangs
+    if(!client_.waitForResult(goal.timeout + timeout_padding_))
+    {
+      //make sure to cancel the goal the server is pursuing
+      client_.cancelGoal();
+      throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.");
+    }
+
+    if(client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
+      throw tf2::TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
+
+    //process the result for errors and return it
+    return processResult(*client_.getResult());
+  }
+
+  geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const
+  {
+    //if there's no error, then we'll just return the transform
+    if(result.error.error != result.error.NO_ERROR){
+      //otherwise, we'll have to throw the appropriate exception
+      if(result.error.error == result.error.LOOKUP_ERROR)
+        throw tf2::LookupException(result.error.error_string);
+
+      if(result.error.error == result.error.CONNECTIVITY_ERROR)
+        throw tf2::ConnectivityException(result.error.error_string);
+
+      if(result.error.error == result.error.EXTRAPOLATION_ERROR)
+        throw tf2::ExtrapolationException(result.error.error_string);
+
+      if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
+        throw tf2::InvalidArgumentException(result.error.error_string);
+
+      if(result.error.error == result.error.TIMEOUT_ERROR)
+        throw tf2::TimeoutException(result.error.error_string);
+
+      throw tf2::TransformException(result.error.error_string);
+    }
+
+    return result.transform;
+  }
+
+  bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame, 
+        const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
+  {
+    try
+    {
+      lookupTransform(target_frame, source_frame, time, timeout);
+      return true;
+    }
+    catch(tf2::TransformException& ex)
+    {
+      if(errstr)
+      { 
+        errstr->clear();
+        *errstr = ex.what();
+      }
+      return false;
+    }
+  }
+
+  bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time,
+        const std::string& source_frame, const ros::Time& source_time,
+        const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
+  {
+    try
+    {
+      lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
+      return true;
+    }
+    catch(tf2::TransformException& ex)
+    {
+      if(errstr)
+      {
+        errstr->clear();
+        *errstr = ex.what();
+      }
+      return false;
+    }
+  }
+};

navigations/geometry2/tf2_ros/src/buffer_server.cpp

@@ -0,0 +1,222 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#include <tf2_ros/buffer_server.h>
+
+namespace tf2_ros
+{
+  BufferServer::BufferServer(const Buffer& buffer, const std::string& ns, bool auto_start, ros::Duration check_period): 
+    buffer_(buffer),
+    server_(ros::NodeHandle(),
+            ns,
+            boost::bind(&BufferServer::goalCB, this, boost::placeholders::_1),
+            boost::bind(&BufferServer::cancelCB, this, boost::placeholders::_1),
+            auto_start)
+  {
+    ros::NodeHandle n;
+    check_timer_ = n.createTimer(check_period, boost::bind(&BufferServer::checkTransforms, this, boost::placeholders::_1));
+  }
+
+  void BufferServer::checkTransforms(const ros::TimerEvent& e)
+  {
+    (void) e; //Unused
+    boost::mutex::scoped_lock l(mutex_);
+    for(std::list<GoalInfo>::iterator it = active_goals_.begin(); it != active_goals_.end();)
+    {
+      GoalInfo& info = *it;
+
+      //we want to lookup a transform if the time on the goal
+      //has expired, or a transform is available
+      if(canTransform(info.handle) || info.end_time < ros::Time::now())
+      {
+        tf2_msgs::LookupTransformResult result;
+
+        //try to populate the result, catching exceptions if they occur
+        try
+        {
+          result.transform = lookupTransform(info.handle);
+        }
+        catch (tf2::ConnectivityException &ex)
+        {
+          result.error.error = result.error.CONNECTIVITY_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::LookupException &ex)
+        {
+          result.error.error = result.error.LOOKUP_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::ExtrapolationException &ex)
+        {
+          result.error.error = result.error.EXTRAPOLATION_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::InvalidArgumentException &ex)
+        {
+          result.error.error = result.error.INVALID_ARGUMENT_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::TimeoutException &ex)
+        {
+          result.error.error = result.error.TIMEOUT_ERROR;
+          result.error.error_string = ex.what();
+        }
+        catch (tf2::TransformException &ex)
+        {
+          result.error.error = result.error.TRANSFORM_ERROR;
+          result.error.error_string = ex.what();
+        }
+
+        //make sure to pass the result to the client
+        //even failed transforms are considered a success
+        //since the request was successfully processed
+        info.handle.setSucceeded(result);
+        it = active_goals_.erase(it);
+      }
+      else
+        ++it;
+    }
+  }
+
+  void BufferServer::cancelCB(GoalHandle gh)
+  {
+    boost::mutex::scoped_lock l(mutex_);
+    //we need to find the goal in the list and remove it... also setting it as canceled
+    //if its not in the list, we won't do anything since it will have already been set
+    //as completed
+    for(std::list<GoalInfo>::iterator it = active_goals_.begin(); it != active_goals_.end();)
+    {
+      GoalInfo& info = *it;
+      if(info.handle == gh)
+      {
+        info.handle.setCanceled();
+        it = active_goals_.erase(it);
+        return;
+      }
+      else
+        ++it;
+    }
+  }
+
+  void BufferServer::goalCB(GoalHandle gh)
+  {
+    //we'll accept all goals we get
+    gh.setAccepted();
+
+    //if the transform isn't immediately available, we'll push it onto our list to check
+    //along with the time that the goal will end
+    GoalInfo goal_info;
+    goal_info.handle = gh;
+    goal_info.end_time = ros::Time::now() + gh.getGoal()->timeout;
+
+    //we can do a quick check here to see if the transform is valid
+    //we'll also do this if the end time has been reached 
+    if(canTransform(gh) || goal_info.end_time <= ros::Time::now())
+    {
+      tf2_msgs::LookupTransformResult result;
+      try
+      {
+        result.transform = lookupTransform(gh);
+      }
+      catch (tf2::ConnectivityException &ex)
+      {
+        result.error.error = result.error.CONNECTIVITY_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::LookupException &ex)
+      {
+        result.error.error = result.error.LOOKUP_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::ExtrapolationException &ex)
+      {
+        result.error.error = result.error.EXTRAPOLATION_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::InvalidArgumentException &ex)
+      {
+        result.error.error = result.error.INVALID_ARGUMENT_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::TimeoutException &ex)
+      {
+        result.error.error = result.error.TIMEOUT_ERROR;
+        result.error.error_string = ex.what();
+      }
+      catch (tf2::TransformException &ex)
+      {
+        result.error.error = result.error.TRANSFORM_ERROR;
+        result.error.error_string = ex.what();
+      }
+
+      gh.setSucceeded(result);
+      return;
+    }
+
+    boost::mutex::scoped_lock l(mutex_);
+    active_goals_.push_back(goal_info);
+  }
+
+  bool BufferServer::canTransform(GoalHandle gh)
+  {
+    const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
+
+    //check whether we need to used the advanced or simple api
+    if(!goal->advanced)
+      return buffer_.canTransform(goal->target_frame, goal->source_frame, goal->source_time);
+
+    return buffer_.canTransform(goal->target_frame, goal->target_time, 
+        goal->source_frame, goal->source_time, goal->fixed_frame);
+  }
+
+  geometry_msgs::TransformStamped BufferServer::lookupTransform(GoalHandle gh)
+  {
+    const tf2_msgs::LookupTransformGoal::ConstPtr& goal = gh.getGoal();
+
+    //check whether we need to used the advanced or simple api
+    if(!goal->advanced)
+      return buffer_.lookupTransform(goal->target_frame, goal->source_frame, goal->source_time);
+
+    return buffer_.lookupTransform(goal->target_frame, goal->target_time, 
+        goal->source_frame, goal->source_time, goal->fixed_frame);
+  }
+
+  void BufferServer::start()
+  {
+    server_.start();
+  }
+
+};

navigations/geometry2/tf2_ros/src/buffer_server_main.cpp

@@ -0,0 +1,71 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2009, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Willow Garage, Inc. nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Wim Meeussen
+*********************************************************************/
+#include <tf2_ros/buffer_server.h>
+#include <tf2_ros/transform_listener.h>
+#include <ros/ros.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "tf_buffer");
+  ros::NodeHandle nh;  
+
+  double buffer_size;
+  nh.param("buffer_size", buffer_size, 120.0);
+
+  bool publish_frame_service;
+  nh.param("publish_frame_service", publish_frame_service, false);
+
+  // Legacy behavior re: #209
+  bool use_node_namespace;
+  nh.param("use_node_namespace", use_node_namespace, false);
+  std::string node_name;
+  if (use_node_namespace)
+  {
+    node_name = ros::this_node::getName();
+  }
+  else
+  {
+    node_name = "tf2_buffer_server";
+  }
+
+  tf2_ros::Buffer buffer_core(ros::Duration(buffer_size), publish_frame_service);
+  tf2_ros::TransformListener listener(buffer_core);
+  tf2_ros::BufferServer buffer_server(buffer_core, node_name , false);
+  buffer_server.start();
+
+  ros::spin();
+}

navigations/geometry2/tf2_ros/src/static_transform_broadcaster.cpp

@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/** \author Tully Foote */
+
+
+#include "ros/ros.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/static_transform_broadcaster.h"
+#include <algorithm>
+
+namespace tf2_ros {
+
+StaticTransformBroadcaster::StaticTransformBroadcaster()
+{
+  publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf_static", 100, true);
+};
+
+void StaticTransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> & msgtf)
+{
+  for (const geometry_msgs::TransformStamped& input : msgtf)
+  {
+    auto predicate = [&input](const geometry_msgs::TransformStamped existing) {
+      return input.child_frame_id == existing.child_frame_id;
+    };
+    auto existing = std::find_if(net_message_.transforms.begin(), net_message_.transforms.end(), predicate);
+
+    if (existing != net_message_.transforms.end())
+      *existing = input;
+    else
+      net_message_.transforms.push_back(input);
+  }
+
+  publisher_.publish(net_message_);
+}
+
+}

navigations/geometry2/tf2_ros/src/static_transform_broadcaster_program.cpp

@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <cstdio>
+#include <tf2/LinearMath/Quaternion.h>
+#include "tf2_ros/static_transform_broadcaster.h"
+
+
+bool validateXmlRpcTf(XmlRpc::XmlRpcValue tf_data) {
+  // Validate a TF stored in XML RPC format: ensures the appropriate fields
+  // exist. Note this does not check data types.
+  return tf_data.hasMember("child_frame_id") &&
+         tf_data.hasMember("header") &&
+         tf_data["header"].hasMember("frame_id") &&
+         tf_data.hasMember("transform") &&
+         tf_data["transform"].hasMember("translation") &&
+         tf_data["transform"]["translation"].hasMember("x") &&
+         tf_data["transform"]["translation"].hasMember("y") &&
+         tf_data["transform"]["translation"].hasMember("z") &&
+         tf_data["transform"].hasMember("rotation") &&
+         tf_data["transform"]["rotation"].hasMember("x") &&
+         tf_data["transform"]["rotation"].hasMember("y") &&
+         tf_data["transform"]["rotation"].hasMember("z") &&
+         tf_data["transform"]["rotation"].hasMember("w");
+};
+
+int main(int argc, char ** argv)
+{
+  //Initialize ROS
+  ros::init(argc, argv,"static_transform_publisher", ros::init_options::AnonymousName);
+  tf2_ros::StaticTransformBroadcaster broadcaster;
+  geometry_msgs::TransformStamped msg;
+
+  if(argc == 10)
+  {
+    msg.transform.translation.x = atof(argv[1]);
+    msg.transform.translation.y = atof(argv[2]);
+    msg.transform.translation.z = atof(argv[3]);
+    msg.transform.rotation.x = atof(argv[4]);
+    msg.transform.rotation.y = atof(argv[5]);
+    msg.transform.rotation.z = atof(argv[6]);
+    msg.transform.rotation.w = atof(argv[7]);
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = argv[8];
+    msg.child_frame_id = argv[9];
+  }
+  else if (argc == 9)
+  {
+    msg.transform.translation.x = atof(argv[1]);
+    msg.transform.translation.y = atof(argv[2]);
+    msg.transform.translation.z = atof(argv[3]);
+
+    tf2::Quaternion quat;
+    quat.setRPY(atof(argv[6]), atof(argv[5]), atof(argv[4]));
+    msg.transform.rotation.x = quat.x();
+    msg.transform.rotation.y = quat.y();
+    msg.transform.rotation.z = quat.z();
+    msg.transform.rotation.w = quat.w();
+
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = argv[7];
+    msg.child_frame_id = argv[8];
+  }
+  else if (argc == 2) {
+    const std::string param_name = argv[1];
+    ROS_INFO_STREAM("Looking for TF in parameter: " << param_name);
+    XmlRpc::XmlRpcValue tf_data;
+
+    if (!ros::param::has(param_name) || !ros::param::get(param_name, tf_data)) {
+      ROS_FATAL_STREAM("Could not read TF from parameter server: " << param_name);
+      return -1;
+    }
+
+    // Check that all required members are present & of the right type.
+    if (!validateXmlRpcTf(tf_data)) {
+      ROS_FATAL_STREAM("Could not validate XmlRpcC for TF data: " << tf_data);
+      return -1;
+    }
+
+    msg.transform.translation.x = (double) tf_data["transform"]["translation"]["x"];
+    msg.transform.translation.y = (double) tf_data["transform"]["translation"]["y"];
+    msg.transform.translation.z = (double) tf_data["transform"]["translation"]["z"];
+    msg.transform.rotation.x = (double) tf_data["transform"]["rotation"]["x"];
+    msg.transform.rotation.y = (double) tf_data["transform"]["rotation"]["y"];
+    msg.transform.rotation.z = (double) tf_data["transform"]["rotation"]["z"];
+    msg.transform.rotation.w = (double) tf_data["transform"]["rotation"]["w"];
+    msg.header.stamp = ros::Time::now();
+    msg.header.frame_id = (std::string) tf_data["header"]["frame_id"];
+    msg.child_frame_id = (std::string) tf_data["child_frame_id"];
+  }
+  else
+  {
+    printf("A command line utility for manually sending a transform.\n");
+    //printf("It will periodicaly republish the given transform. \n");
+    printf("Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \n");
+    printf("OR \n");
+    printf("Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id \n");
+    printf("OR \n");
+    printf("Usage: static_transform_publisher /param_name \n");
+    printf("\nThis transform is the transform of the coordinate frame from frame_id into the coordinate frame \n");
+    printf("of the child_frame_id.  \n");
+    ROS_ERROR("static_transform_publisher exited due to not having the right number of arguments");
+    return -1;
+  }
+
+  // Checks: frames should not be the same.
+  if (msg.header.frame_id == msg.child_frame_id)
+  {
+    ROS_FATAL("target_frame and source frame are the same (%s, %s) this cannot work",
+              msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
+    return 1;
+  }
+
+  broadcaster.sendTransform(msg);
+  ROS_INFO("Spinning until killed publishing %s to %s",
+           msg.header.frame_id.c_str(), msg.child_frame_id.c_str());
+  ros::spin();
+  return 0;
+};

navigations/geometry2/tf2_ros/src/tf2_ros/__init__.py

@@ -0,0 +1,44 @@
+#! /usr/bin/python
+#***********************************************************
+#* Software License Agreement (BSD License)
+#*
+#*  Copyright (c) 2009, Willow Garage, Inc.
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of Willow Garage, Inc. nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#*
+#* Author: Eitan Marder-Eppstein
+#***********************************************************
+from __future__ import absolute_import
+from tf2_py import *
+from .buffer_interface import *
+from .buffer import *
+from .buffer_client import *
+from .transform_listener import *
+from .transform_broadcaster import *
+from .static_transform_broadcaster import *

navigations/geometry2/tf2_ros/src/tf2_ros/buffer.py

@@ -0,0 +1,162 @@
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+# 
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# 
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+# 
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# author: Wim Meeussen
+
+import rospy
+import tf2_py as tf2
+import tf2_ros
+from tf2_msgs.srv import FrameGraph, FrameGraphResponse
+import rosgraph.masterapi
+
+DEFAULT_CAN_TRANSFORM_POLLING_SCALE = 0.01
+
+class Buffer(tf2.BufferCore, tf2_ros.BufferInterface):
+    """
+    Standard implementation of the :class:`tf2_ros.BufferInterface` abstract data type.
+
+    Inherits from :class:`tf2_ros.buffer_interface.BufferInterface` and :class:`tf2.BufferCore`.
+
+    Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests
+    with a response containing a :class:`tf2_msgs.FrameGraph` representing the relationship of
+    known frames. 
+    """
+
+    def __init__(self, cache_time = None, debug = True):
+        """
+        .. function:: __init__(cache_time = None, debug = True)
+
+            Constructor.
+
+            :param cache_time: (Optional) How long to retain past information in BufferCore.
+            :param debug: (Optional) If true, check if another tf2_frames service has been advertised.
+        """
+        if cache_time != None:
+            tf2.BufferCore.__init__(self, cache_time)
+        else:
+            tf2.BufferCore.__init__(self)
+        tf2_ros.BufferInterface.__init__(self)
+
+        if debug:
+            #Check to see if the service has already been advertised in this node
+            try:
+                m = rosgraph.masterapi.Master(rospy.get_name())
+                m.lookupService('~tf2_frames')
+            except (rosgraph.masterapi.Error, rosgraph.masterapi.Failure):   
+                self.frame_server = rospy.Service('~tf2_frames', FrameGraph, self.__get_frames)
+
+        self.CAN_TRANSFORM_POLLING_SCALE = DEFAULT_CAN_TRANSFORM_POLLING_SCALE
+
+    def __get_frames(self, req):
+       return FrameGraphResponse(self.all_frames_as_yaml()) 
+
+    def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+
+        self.can_transform(target_frame, source_frame, time, timeout)
+        return self.lookup_transform_core(target_frame, source_frame, time)
+
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame using the advanced API.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        self.can_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout)
+        return self.lookup_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)
+
+
+    def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0), return_debug_tuple=False):
+        """
+        Check if a transform from the source frame to the target frame is possible.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        if timeout != rospy.Duration(0.0):
+            start_time = rospy.Time.now()
+            polling_interval = timeout * self.CAN_TRANSFORM_POLLING_SCALE
+            while (rospy.Time.now() < start_time + timeout and 
+                   not self.can_transform_core(target_frame, source_frame, time)[0] and
+                   (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them
+                rospy.sleep(polling_interval)
+        core_result = self.can_transform_core(target_frame, source_frame, time)
+        if return_debug_tuple:
+            return core_result
+        return core_result[0]
+
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0),
+
+                           return_debug_tuple=False):
+        """
+        Check if a transform from the source frame to the target frame is possible (advanced API).
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        if timeout != rospy.Duration(0.0):
+            start_time = rospy.Time.now()
+            polling_interval = timeout * self.CAN_TRANSFORM_POLLING_SCALE
+            while (rospy.Time.now() < start_time + timeout and 
+                   not self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)[0] and
+                   (rospy.Time.now()+rospy.Duration(3.0)) >= start_time): # big jumps in time are likely bag loops, so break for them
+                rospy.sleep(polling_interval)
+        core_result = self.can_transform_full_core(target_frame, target_time, source_frame, source_time, fixed_frame)
+        if return_debug_tuple:
+            return core_result
+        return core_result[0]
+

navigations/geometry2/tf2_ros/src/tf2_ros/buffer_client.py

@@ -0,0 +1,196 @@
+#! /usr/bin/python
+#***********************************************************
+#* Software License Agreement (BSD License)
+#*
+#*  Copyright (c) 2009, Willow Garage, Inc.
+#*  All rights reserved.
+#*
+#*  Redistribution and use in source and binary forms, with or without
+#*  modification, are permitted provided that the following conditions
+#*  are met:
+#*
+#*   * Redistributions of source code must retain the above copyright
+#*     notice, this list of conditions and the following disclaimer.
+#*   * Redistributions in binary form must reproduce the above
+#*     copyright notice, this list of conditions and the following
+#*     disclaimer in the documentation and/or other materials provided
+#*     with the distribution.
+#*   * Neither the name of Willow Garage, Inc. nor the names of its
+#*     contributors may be used to endorse or promote products derived
+#*     from this software without specific prior written permission.
+#*
+#*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+#*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+#*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+#*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+#*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+#*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+#*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+#*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+#*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+#*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#*  POSSIBILITY OF SUCH DAMAGE.
+#* 
+#* Author: Eitan Marder-Eppstein
+#***********************************************************
+import rospy
+import actionlib
+import tf2_py as tf2
+import tf2_ros
+
+from tf2_msgs.msg import LookupTransformAction, LookupTransformGoal
+from actionlib_msgs.msg import GoalStatus
+
+class BufferClient(tf2_ros.BufferInterface):
+    """
+    Action client-based implementation of BufferInterface.
+    """
+    def __init__(self, ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0)):
+        """
+        .. function:: __init__(ns, check_frequency = None, timeout_padding = rospy.Duration.from_sec(2.0))
+
+            Constructor.
+
+            :param ns: The namespace in which to look for a BufferServer.
+            :param check_frequency: How frequently to check for updates to known transforms.
+            :param timeout_padding: A constant timeout to add to blocking calls.
+        """
+        tf2_ros.BufferInterface.__init__(self)
+        self.client = actionlib.SimpleActionClient(ns, LookupTransformAction)
+        self.timeout_padding = timeout_padding
+
+        if check_frequency is not None:
+            rospy.logwarn('Argument check_frequency is deprecated and should not be used.')
+
+    def wait_for_server(self, timeout = rospy.Duration()):
+        """
+        Block until the action server is ready to respond to requests. 
+
+        :param timeout: Time to wait for the server.
+        :return: True if the server is ready, false otherwise.
+        :rtype: bool
+        """
+        return self.client.wait_for_server(timeout)
+
+    # lookup, simple api 
+    def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        goal = LookupTransformGoal()
+        goal.target_frame = target_frame;
+        goal.source_frame = source_frame;
+        goal.source_time = time;
+        goal.timeout = timeout;
+        goal.advanced = False;
+
+        return self.__process_goal(goal)
+
+    # lookup, advanced api 
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame using the advanced API.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        goal = LookupTransformGoal()
+        goal.target_frame = target_frame;
+        goal.source_frame = source_frame;
+        goal.source_time = source_time;
+        goal.timeout = timeout;
+        goal.target_time = target_time;
+        goal.fixed_frame = fixed_frame;
+        goal.advanced = True;
+
+        return self.__process_goal(goal)
+
+    # can, simple api
+    def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        try:
+            self.lookup_transform(target_frame, source_frame, time, timeout)
+            return True
+        except tf2.TransformException:
+            return False
+
+    
+    # can, advanced api
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible (advanced API).
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param return_debug_type: (Optional) If true, return a tuple representing debug information.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        try:
+            self.lookup_transform_full(target_frame, target_time, source_frame, source_time, fixed_frame, timeout)
+            return True
+        except tf2.TransformException:
+            return False
+
+    def __process_goal(self, goal):
+        self.client.send_goal(goal)
+
+        if not self.client.wait_for_result(goal.timeout + self.timeout_padding):
+            #This shouldn't happen, but could in rare cases where the server hangs
+            raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server")
+
+        if self.client.get_state() != GoalStatus.SUCCEEDED:
+            raise tf2.TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.")
+
+        return self.__process_result(self.client.get_result())
+
+    def __process_result(self, result):
+        if not result:
+            raise tf2.TransformException("The BufferServer returned None for result!  Something is likely wrong with the server.")
+        if not result.error:
+            raise tf2.TransformException("The BufferServer returned None for result.error!  Something is likely wrong with the server.")
+        if result.error.error != result.error.NO_ERROR:
+            if result.error.error == result.error.LOOKUP_ERROR:
+                raise tf2.LookupException(result.error.error_string)
+            if result.error.error == result.error.CONNECTIVITY_ERROR:
+                raise tf2.ConnectivityException(result.error.error_string)
+            if result.error.error == result.error.EXTRAPOLATION_ERROR:
+                raise tf2.ExtrapolationException(result.error.error_string)
+            if result.error.error == result.error.INVALID_ARGUMENT_ERROR:
+                raise tf2.InvalidArgumentException(result.error.error_string)
+            if result.error.error == result.error.TIMEOUT_ERROR:
+                raise tf2.TimeoutException(result.error.error_string)
+
+            raise tf2.TransformException(result.error.error_string)
+
+        return result.transform

navigations/geometry2/tf2_ros/src/tf2_ros/buffer_interface.py

@@ -0,0 +1,251 @@
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+# 
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# 
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+# 
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# author: Wim Meeussen
+
+from __future__ import print_function
+
+import rospy
+import tf2_py as tf2
+import tf2_ros
+from copy import deepcopy
+from std_msgs.msg import Header
+
+class BufferInterface:
+    """
+    Abstract interface for wrapping the Python bindings for the tf2 library in
+    a ROS-based convenience API.
+    Implementations include :class:tf2_ros.buffer.Buffer and
+    :class:tf2_ros.buffer_client.BufferClient.
+    """
+    def __init__(self):
+        self.registration = tf2_ros.TransformRegistration()
+
+    # transform, simple api
+    def transform(self, object_stamped, target_frame, timeout=rospy.Duration(0.0), new_type = None):
+        """
+        Transform an input into the target frame.
+
+        The input must be a known transformable type (by way of the tf2 data type conversion interface).
+
+        If new_type is not None, the type specified must have a valid conversion from the input type,
+        else the function will raise an exception.
+
+        :param object_stamped: The timestamped object the transform.
+        :param target_frame: Name of the frame to transform the input into.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param new_type: (Optional) Type to convert the object to.
+        :return: The transformed, timestamped output, possibly converted to a new type.
+        """
+        do_transform = self.registration.get(type(object_stamped))
+        res = do_transform(object_stamped, self.lookup_transform(target_frame, object_stamped.header.frame_id,
+                                                                 object_stamped.header.stamp, timeout))
+        if not new_type:
+            return res
+
+        return convert(res, new_type)
+
+    # transform, advanced api
+    def transform_full(self, object_stamped, target_frame, target_time, fixed_frame, timeout=rospy.Duration(0.0), new_type = None):
+        """
+        Transform an input into the target frame (advanced API).
+
+        The input must be a known transformable type (by way of the tf2 data type conversion interface).
+
+        If new_type is not None, the type specified must have a valid conversion from the input type,
+        else the function will raise an exception.
+
+        This function follows the advanced API, which allows tranforming between different time points,
+        as well as specifying a frame to be considered fixed in time.
+
+        :param object_stamped: The timestamped object the transform.
+        :param target_frame: Name of the frame to transform the input into.
+        :param target_time: Time to transform the input into.
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :param new_type: (Optional) Type to convert the object to.
+        :return: The transformed, timestamped output, possibly converted to a new type.
+        """
+        do_transform = self.registration.get(type(object_stamped))
+        res = do_transform(object_stamped, self.lookup_transform_full(target_frame, target_time,
+                                                                     object_stamped.header.frame_id, object_stamped.header.stamp, 
+                                                                     fixed_frame, timeout))
+        if not new_type:
+            return res
+
+        return convert(res, new_type)
+
+    def lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame.
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        raise NotImplementedException()
+
+    def lookup_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Get the transform from the source frame to the target frame using the advanced API.
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: The transform between the frames.
+        :rtype: :class:`geometry_msgs.msg.TransformStamped`
+        """
+        raise NotImplementedException()        
+
+    # can, simple api
+    def can_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible.
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param source_frame: Name of the input frame.
+        :param time: The time at which to get the transform. (0 will get the latest) 
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        raise NotImplementedException()        
+    
+    # can, advanced api
+    def can_transform_full(self, target_frame, target_time, source_frame, source_time, fixed_frame, timeout=rospy.Duration(0.0)):
+        """
+        Check if a transform from the source frame to the target frame is possible (advanced API).
+
+        Must be implemented by a subclass of BufferInterface.
+
+        :param target_frame: Name of the frame to transform into.
+        :param target_time: The time to transform to. (0 will get the latest) 
+        :param source_frame: Name of the input frame.
+        :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) 
+        :param fixed_frame: Name of the frame to consider constant in time.
+        :param timeout: (Optional) Time to wait for the target frame to become available.
+        :return: True if the transform is possible, false otherwise.
+        :rtype: bool
+        """
+        raise NotImplementedException()        
+
+
+def Stamped(obj, stamp, frame_id):
+    obj.header = Header(frame_id=frame_id, stamp=stamp)
+    return obj
+
+
+
+class TypeException(Exception):
+    """
+    Raised when an unexpected type is received while registering a transform
+    in :class:`tf2_ros.buffer_interface.BufferInterface`.
+    """
+    def __init__(self, errstr):
+        self.errstr = errstr
+
+class NotImplementedException(Exception):
+    """
+    Raised when can_transform or lookup_transform is not implemented in a
+    subclass of :class:`tf2_ros.buffer_interface.BufferInterface`.
+    """
+    def __init__(self):
+        self.errstr = 'CanTransform or LookupTransform not implemented'
+
+
+class TransformRegistration():
+    __type_map = {}
+    
+    def print_me(self):
+        print(TransformRegistration.__type_map)
+
+    def add(self, key, callback):
+        TransformRegistration.__type_map[key] = callback
+
+    def get(self, key):
+        if not key in TransformRegistration.__type_map:
+            raise TypeException('Type %s if not loaded or supported'% str(key))
+        else:
+            return TransformRegistration.__type_map[key]
+
+class ConvertRegistration():
+    __to_msg_map = {}
+    __from_msg_map = {}
+    __convert_map = {}
+    
+    def add_from_msg(self, key, callback):
+        ConvertRegistration.__from_msg_map[key] = callback
+
+    def add_to_msg(self, key, callback):
+        ConvertRegistration.__to_msg_map[key] = callback
+
+    def add_convert(self, key, callback):
+        ConvertRegistration.__convert_map[key] = callback
+
+    def get_from_msg(self, key):
+        if not key in ConvertRegistration.__from_msg_map:
+            raise TypeException('Type %s if not loaded or supported'% str(key))
+        else:
+            return ConvertRegistration.__from_msg_map[key]
+
+    def get_to_msg(self, key):
+        if not key in ConvertRegistration.__to_msg_map:
+            raise TypeException('Type %s if not loaded or supported'%str(key))
+        else:
+            return ConvertRegistration.__to_msg_map[key]
+
+    def get_convert(self, key):
+        if not key in ConvertRegistration.__convert_map:
+            raise TypeException("Type %s if not loaded or supported" % str(key))
+        else:
+            return ConvertRegistration.__convert_map[key]
+
+def convert(a, b_type):
+    c = ConvertRegistration()
+    #check if an efficient conversion function between the types exists
+    try:
+        f = c.get_convert((type(a), b_type))
+        return f(a)
+    except TypeException:
+        if type(a) == b_type:
+            return deepcopy(a)
+
+        f_to = c.get_to_msg(type(a))
+        f_from = c.get_from_msg(b_type)
+        return f_from(f_to(a))

navigations/geometry2/tf2_ros/src/tf2_ros/static_transform_broadcaster.py

@@ -0,0 +1,51 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+
+
+class StaticTransformBroadcaster(object):
+    """
+    :class:`StaticTransformBroadcaster` is a convenient way to send static transformation on the ``"/tf_static"`` message topic.
+    """
+
+    def __init__(self):
+        self.pub_tf = rospy.Publisher("/tf_static", TFMessage, queue_size=100, latch=True)
+
+    def sendTransform(self, transform):
+        if not isinstance(transform, list):
+            transform = [transform]
+        self.pub_tf.publish(TFMessage(transform))
+
+

navigations/geometry2/tf2_ros/src/tf2_ros/transform_broadcaster.py

@@ -0,0 +1,56 @@
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of the Willow Garage nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rospy
+from tf2_msgs.msg import TFMessage
+from geometry_msgs.msg import TransformStamped
+
+
+class TransformBroadcaster:
+    """
+    :class:`TransformBroadcaster` is a convenient way to send transformation updates on the ``"/tf"`` message topic.
+    """
+
+    def __init__(self):
+        self.pub_tf = rospy.Publisher("/tf", TFMessage, queue_size=100)
+
+    def sendTransform(self, transform):
+        """
+        Send a transform, or a list of transforms, to the Buffer associated with this TransformBroadcaster.
+
+        :param transform: A transform or list of transforms to send.
+        """
+        if not isinstance(transform, list):
+            transform = [transform]
+        self.pub_tf.publish(TFMessage(transform))
+
+

navigations/geometry2/tf2_ros/src/tf2_ros/transform_listener.py

@@ -0,0 +1,89 @@
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+# 
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# 
+#     * Redistributions of source code must retain the above copyright
+#       notice, this list of conditions and the following disclaimer.
+#     * Redistributions in binary form must reproduce the above copyright
+#       notice, this list of conditions and the following disclaimer in the
+#       documentation and/or other materials provided with the distribution.
+#     * Neither the name of the Willow Garage, Inc. nor the names of its
+#       contributors may be used to endorse or promote products derived from
+#       this software without specific prior written permission.
+# 
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# author: Wim Meeussen
+
+import threading
+
+import rospy
+from tf2_msgs.msg import TFMessage
+
+
+class TransformListener:
+    """
+    :class:`TransformListener` is a convenient way to listen for coordinate frame transformation info.
+    This class takes an object that instantiates the :class:`BufferInterface` interface, to which
+    it propagates changes to the tf frame graph.
+    """
+    def __init__(self, buffer, queue_size=None, buff_size=65536, tcp_nodelay=False):
+        """
+        .. function:: __init__(buffer, queue_size=None, buff_size=65536, tcp_nodelay=False):
+
+            Constructor.
+
+            :param buffer: The buffer to propagate changes to when tf info updates.
+            :param queue_size: Maximum number of messages to receive at a time. This will generally be 1 or None (infinite, default). buff_size should be increased if this parameter is set as incoming data still needs to sit in the incoming buffer before being discarded. Setting queue_size buff_size to a non-default value affects all subscribers to this topic in this process.
+            :param buff_size: Incoming message buffer size in bytes. If queue_size is set, this should be set to a number greater than the queue_size times the average message size. Setting buff_size to a non-default value affects all subscribers to this topic in this process.
+            :param tcp_nodelay: If True, request TCP_NODELAY from publisher. Use of this option is not generally recommended in most cases as it is better to rely on timestamps in message data. Setting tcp_nodelay to True enables TCP_NODELAY for all subscribers in the same python process.
+        """
+        self.buffer = buffer
+        self.last_update = rospy.Time.now()
+        self.last_update_lock = threading.Lock()
+        self.tf_sub = rospy.Subscriber("/tf", TFMessage, self.callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay)
+        self.tf_static_sub = rospy.Subscriber("/tf_static", TFMessage, self.static_callback, queue_size=queue_size, buff_size=buff_size, tcp_nodelay=tcp_nodelay)
+
+    def __del__(self):
+        self.unregister()
+
+    def unregister(self):
+        """
+        Unregisters all tf subscribers.
+        """
+        self.tf_sub.unregister()
+        self.tf_static_sub.unregister()
+
+    def check_for_reset(self):
+        # Lock to prevent different threads racing on this test and update.
+        # https://github.com/ros/geometry2/issues/341
+        with self.last_update_lock:
+            now = rospy.Time.now()
+            if now < self.last_update:
+                rospy.logwarn("Detected jump back in time of %fs. Clearing TF buffer." % (self.last_update - now).to_sec())
+                self.buffer.clear()
+            self.last_update = now
+
+    def callback(self, data):
+        self.check_for_reset()
+        who = data._connection_header.get('callerid', "default_authority")
+        for transform in data.transforms:
+            self.buffer.set_transform(transform, who)
+
+    def static_callback(self, data):
+        self.check_for_reset()
+        who = data._connection_header.get('callerid', "default_authority")
+        for transform in data.transforms:
+            self.buffer.set_transform_static(transform, who)

navigations/geometry2/tf2_ros/src/transform_broadcaster.cpp

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ * 
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ * 
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ * 
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+
+/** \author Tully Foote */
+
+
+#include "ros/ros.h"
+#include "tf2_msgs/TFMessage.h"
+#include "tf2_ros/transform_broadcaster.h"
+
+namespace tf2_ros {
+
+TransformBroadcaster::TransformBroadcaster()
+{
+  publisher_ = node_.advertise<tf2_msgs::TFMessage>("/tf", 100);
+};
+
+void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
+{
+  std::vector<geometry_msgs::TransformStamped> v1;
+  v1.push_back(msgtf);
+  sendTransform(v1);
+}
+
+
+void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::TransformStamped> & msgtf)
+{
+  tf2_msgs::TFMessage message;
+  for (std::vector<geometry_msgs::TransformStamped>::const_iterator it = msgtf.begin(); it != msgtf.end(); ++it)
+  {
+    message.transforms.push_back(*it);
+  }
+  publisher_.publish(message);
+}
+
+
+}
+
+

navigations/geometry2/tf2_ros/src/transform_listener.cpp

@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Tully Foote */
+
+#include "tf2_ros/transform_listener.h"
+
+
+using namespace tf2_ros;
+
+
+TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread, ros::TransportHints transport_hints):
+  dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false), transport_hints_(transport_hints)
+{
+  if (spin_thread)
+    initWithThread();
+  else
+    init();
+}
+
+TransformListener::TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread,
+    ros::TransportHints transport_hints)
+: dedicated_listener_thread_(NULL)
+, node_(nh)
+, buffer_(buffer)
+, using_dedicated_thread_(false)
+, transport_hints_(transport_hints)
+{
+  if (spin_thread)
+    initWithThread();
+  else
+    init();
+}
+
+TransformListener::~TransformListener()
+{
+  using_dedicated_thread_ = false;
+  if (dedicated_listener_thread_)
+  {
+    dedicated_listener_thread_->join();
+    delete dedicated_listener_thread_;
+  }
+}
+
+void TransformListener::init()
+{
+  message_subscriber_tf_ = node_.subscribe<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, boost::placeholders::_1), ros::VoidConstPtr(), transport_hints_); ///\todo magic number
+  message_subscriber_tf_static_ = node_.subscribe<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, boost::placeholders::_1)); ///\todo magic number
+}
+
+void TransformListener::initWithThread()
+{
+  using_dedicated_thread_ = true;
+  ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, boost::placeholders::_1), ros::VoidPtr(), &tf_message_callback_queue_); ///\todo magic number
+  ops_tf.transport_hints = transport_hints_;
+  message_subscriber_tf_ = node_.subscribe(ops_tf);
+  
+  ros::SubscribeOptions ops_tf_static = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, boost::placeholders::_1), ros::VoidPtr(), &tf_message_callback_queue_); ///\todo magic number
+  message_subscriber_tf_static_ = node_.subscribe(ops_tf_static);
+
+  dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
+
+  //Tell the buffer we have a dedicated thread to enable timeouts
+  buffer_.setUsingDedicatedThread(true);
+}
+
+
+
+void TransformListener::subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt)
+{
+  subscription_callback_impl(msg_evt, false);
+}
+void TransformListener::static_subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt)
+{
+  subscription_callback_impl(msg_evt, true);
+}
+
+void TransformListener::subscription_callback_impl(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt, bool is_static)
+{
+  ros::Time now = ros::Time::now();
+  if(now < last_update_){
+    ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer.");
+    buffer_.clear();
+  }
+  last_update_ = now;
+
+
+
+  const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage());
+  std::string authority = msg_evt.getPublisherName(); // lookup the authority
+  for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
+  {
+    try
+    {
+      buffer_.setTransform(msg_in.transforms[i], authority, is_static);
+    }
+    
+    catch (tf2::TransformException& ex)
+    {
+      ///\todo Use error reporting
+      std::string temp = ex.what();
+      ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
+    }
+  }
+};
+
+
+
+
+