HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit"

Browse Source
This commit is contained in:
HiepLM
2026-05-28 10:29:58 +07:00
commit 167c52aeb6
2048 changed files with 740251 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

144
navigations/geometry2/tf2_ros/include/tf2_ros/buffer.h Executable file
View File

@@ -0,0 +1,144 @@
/*
* 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 */
#ifndef TF2_ROS_BUFFER_H
#define TF2_ROS_BUFFER_H
#include <tf2_ros/buffer_interface.h>
#include <tf2/buffer_core.h>
#include <tf2_msgs/FrameGraph.h>
#include <ros/ros.h>
#include <tf2/convert.h>
namespace tf2_ros
{
/** \brief Standard implementation of the tf2_ros::BufferInterface abstract data type.
*
* Inherits tf2_ros::BufferInterface and tf2::BufferCore.
* Stores known frames and optionally offers a ROS service, "tf2_frames", which responds to client requests
* with a response containing a tf2_msgs::FrameGraph representing the relationship of known frames.
*/
class Buffer: public BufferInterface, public tf2::BufferCore
{
public:
using tf2::BufferCore::lookupTransform;
using tf2::BufferCore::canTransform;
/**
* @brief Constructor for a Buffer object
* @param cache_time How long to keep a history of transforms
* @param debug Whether to advertise the tf2_frames service that exposes debugging information from the buffer
* @return
*/
Buffer(ros::Duration cache_time = ros::Duration(BufferCore::DEFAULT_CACHE_TIME), bool debug = false);
/** \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
* \param timeout How long to block before failing
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
virtual geometry_msgs::TransformStamped
lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration timeout) const;
/** \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed
* \param target_time The time to which the data should be transformed. (0 will get the latest)
* \param source_frame The frame where the data originated
* \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \param timeout How long to block before failing
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
virtual geometry_msgs::TransformStamped
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;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param source_frame The frame from which to transform
* \param target_time The time at which to transform
* \param timeout How long to block before failing
* \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
virtual bool
canTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& target_time, const ros::Duration timeout, std::string* errstr = NULL) const;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param target_time The time into which to transform
* \param source_frame The frame from which to transform
* \param source_time The time from which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time
* \param timeout How long to block before failing
* \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
virtual bool
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 = NULL) const;
private:
bool getFrames(tf2_msgs::FrameGraph::Request& req, tf2_msgs::FrameGraph::Response& res) ;
// conditionally error if dedicated_thread unset.
bool checkAndErrorDedicatedThreadPresent(std::string* errstr) const;
ros::ServiceServer frames_server_;
}; // class
static const std::string threading_error = "Do not call canTransform or lookupTransform with a timeout unless you are using another thread for populating data. Without a dedicated thread it will always timeout. If you have a separate thread servicing tf messages, call setUsingDedicatedThread(true) on your Buffer instance.";
} // namespace
#endif // TF2_ROS_BUFFER_H

139
navigations/geometry2/tf2_ros/include/tf2_ros/buffer_client.h Executable file
View File

@@ -0,0 +1,139 @@
/*********************************************************************
*
* 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
*********************************************************************/
#ifndef TF2_ROS_BUFFER_CLIENT_H_
#define TF2_ROS_BUFFER_CLIENT_H_
#include <tf2_ros/buffer_interface.h>
#include <actionlib/client/simple_action_client.h>
#include <tf2_msgs/LookupTransformAction.h>
namespace tf2_ros
{
/** \brief Action client-based implementation of the tf2_ros::BufferInterface abstract data type.
*
* BufferClient uses actionlib to coordinate waiting for available transforms.
*
* You can use this class with a tf2_ros::BufferServer and tf2_ros::TransformListener in a separate process.
*/
class BufferClient : public BufferInterface
{
public:
typedef actionlib::SimpleActionClient<tf2_msgs::LookupTransformAction> LookupActionClient;
/** \brief BufferClient constructor
* \param ns The namespace in which to look for a BufferServer
* \param check_frequency Deprecated, not used anymore
* \param timeout_padding The amount of time to allow passed the desired timeout on the client side for communication lag
*/
BufferClient(std::string ns, double check_frequency = 10.0, ros::Duration timeout_padding = ros::Duration(2.0));
/** \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
* \param timeout How long to block before failing
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
virtual geometry_msgs::TransformStamped
lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0)) const;
/** \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed
* \param target_time The time to which the data should be transformed. (0 will get the latest)
* \param source_frame The frame where the data originated
* \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \param timeout How long to block before failing
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
virtual geometry_msgs::TransformStamped
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 = ros::Duration(0.0)) const;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param source_frame The frame from which to transform
* \param time The time at which to transform
* \param timeout How long to block before failing
* \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
virtual bool
canTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration timeout = ros::Duration(0.0), std::string* errstr = NULL) const;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param target_time The time into which to transform
* \param source_frame The frame from which to transform
* \param source_time The time from which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time
* \param timeout How long to block before failing
* \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
virtual bool
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 = ros::Duration(0.0), std::string* errstr = NULL) const;
/** \brief 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.
*/
bool waitForServer(const ros::Duration& timeout = ros::Duration(0))
{
return client_.waitForServer(timeout);
}
private:
geometry_msgs::TransformStamped processGoal(const tf2_msgs::LookupTransformGoal& goal) const;
geometry_msgs::TransformStamped processResult(const tf2_msgs::LookupTransformResult& result) const;
mutable LookupActionClient client_;
double check_frequency_;
ros::Duration timeout_padding_;
};
};
#endif

267
navigations/geometry2/tf2_ros/include/tf2_ros/buffer_interface.h Executable file
View File

@@ -0,0 +1,267 @@
/*
* 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 */
#ifndef TF2_ROS_BUFFER_INTERFACE_H
#define TF2_ROS_BUFFER_INTERFACE_H
#include <tf2/buffer_core.h>
#include <tf2/transform_datatypes.h>
#include <tf2/exceptions.h>
#include <geometry_msgs/TransformStamped.h>
#include <sstream>
#include <tf2/convert.h>
namespace tf2_ros
{
/** \brief Abstract interface for wrapping tf2::BufferCore in a ROS-based API.
* Implementations include tf2_ros::Buffer and tf2_ros::BufferClient.
*/
class BufferInterface
{
public:
/** \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
* \param timeout How long to block before failing
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
virtual geometry_msgs::TransformStamped
lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration timeout) const = 0;
/** \brief Get the transform between two frames by frame ID assuming fixed frame.
* \param target_frame The frame to which data should be transformed
* \param target_time The time to which the data should be transformed. (0 will get the latest)
* \param source_frame The frame where the data originated
* \param source_time The time at which the source_frame should be evaluated. (0 will get the latest)
* \param fixed_frame The frame in which to assume the transform is constant in time.
* \param timeout How long to block before failing
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException
*/
virtual geometry_msgs::TransformStamped
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 = 0;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param source_frame The frame from which to transform
* \param time The time at which to transform
* \param timeout How long to block before failing
* \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
virtual bool
canTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform
* \param target_time The time into which to transform
* \param source_frame The frame from which to transform
* \param source_time The time from which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time
* \param timeout How long to block before failing
* \param errstr A pointer to a string which will be filled with why the transform failed, if not NULL
* \return True if the transform is possible, false otherwise
*/
virtual bool
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 = NULL) const = 0;
/** \brief Transform an input into the target frame.
* This function is templated and can take as input any valid mathematical object that tf knows
* how to apply a transform to, by way of the templated math conversions interface.
* For example, the template type could be a Transform, Pose, Vector, or Quaternion message
* type (as defined in geometry_msgs).
* \tparam T The type of the object to transform.
* \param in The object to transform
* \param out The transformed output, preallocated by the caller.
* \param target_frame The string identifer for the frame to transform into.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
*/
template <class T>
T& transform(const T& in, T& out,
const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
// do the transform
tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
return out;
}
/** \brief Transform an input into the target frame.
* This function is templated and can take as input any valid mathematical object that tf knows
* how to apply a transform to, by way of the templated math conversions interface.
* For example, the template type could be a Transform, Pose, Vector, or Quaternion message
* type (as defined in geometry_msgs).
* \tparam T The type of the object to transform.
* \param in The object to transform.
* \param target_frame The string identifer for the frame to transform into.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
* \return The transformed output.
*/
template <class T>
T transform(const T& in,
const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
T out;
return transform(in, out, target_frame, timeout);
}
/** \brief Transform an input into the target frame and convert to a specified output type.
* It is templated on two types: the type of the input object and the type of the
* transformed output.
* For example, the template types could be Transform, Pose, Vector, or Quaternion messages
* type (as defined in geometry_msgs).
* The function will calculate the transformation and then convert the result into the
* specified output type.
* Compilation will fail if a known conversion does not exist bewteen the two template
* parameters.
* \tparam A The type of the object to transform.
* \tparam B The type of the transformed output.
* \param in The object to transform
* \param out The transformed output, converted to the specified type.
* \param target_frame The string identifer for the frame to transform into.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
* \return The transformed output, converted to the specified type.
*/
template <class A, class B>
B& transform(const A& in, B& out,
const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
A copy = transform(in, target_frame, timeout);
tf2::convert(copy, out);
return out;
}
/** \brief Transform an input into the target frame (advanced).
* This function is templated and can take as input any valid mathematical object that tf knows
* how to apply a transform to, by way of the templated math conversions interface.
* For example, the template type could be a Transform, Pose, Vector, or Quaternion message
* type (as defined in geometry_msgs).
* This function follows the advanced API, which allows transforming between different time
* points, and specifying a fixed frame that does not varying in time.
* \tparam T The type of the object to transform.
* \param in The object to transform
* \param out The transformed output, preallocated by the caller.
* \param target_frame The string identifer for the frame to transform into.
* \param target_time The time into which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
*/
template <class T>
T& transform(const T& in, T& out,
const std::string& target_frame, const ros::Time& target_time,
const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
// do the transform
tf2::doTransform(in, out, lookupTransform(target_frame, target_time,
tf2::getFrameId(in), tf2::getTimestamp(in),
fixed_frame, timeout));
return out;
}
/** \brief Transform an input into the target frame (advanced).
* This function is templated and can take as input any valid mathematical object that tf knows
* how to apply a transform to, by way of the templated math conversions interface.
* For example, the template type could be a Transform, Pose, Vector, or Quaternion message
* type (as defined in geometry_msgs).
* This function follows the advanced API, which allows transforming between different time
* points, and specifying a fixed frame that does not varying in time.
* \tparam T The type of the object to transform.
* \param in The object to transform
* \param target_frame The string identifer for the frame to transform into.
* \param target_time The time into which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
* \return The transformed output.
*/
template <class T>
T transform(const T& in,
const std::string& target_frame, const ros::Time& target_time,
const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
T out;
return transform(in, out, target_frame, target_time, fixed_frame, timeout);
}
/** \brief Transform an input into the target frame and convert to a specified output type (advanced).
* It is templated on two types: the type of the input object and the type of the
* transformed output.
* For example, the template type could be a Transform, Pose, Vector, or Quaternion message
* type (as defined in geometry_msgs).
* The function will calculate the transformation and then convert the result into the
* specified output type.
* Compilation will fail if a known conversion does not exist bewteen the two template
* parameters.
* This function follows the advanced API, which allows transforming between different time
* points, and specifying a fixed frame that does not varying in time.
* \tparam A The type of the object to transform.
* \tparam B The type of the transformed output.
* \param in The object to transform
* \param out The transformed output, converted to the specified output type.
* \param target_frame The string identifer for the frame to transform into.
* \param target_time The time into which to transform
* \param fixed_frame The frame in which to treat the transform as constant in time.
* \param timeout How long to wait for the target frame. Default value is zero (no blocking).
* \return The transformed output, converted to the specified output type.
*/
template <class A, class B>
B& transform(const A& in, B& out,
const std::string& target_frame, const ros::Time& target_time,
const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
{
// do the transform
A copy = transform(in, target_frame, target_time, fixed_frame, timeout);
tf2::convert(copy, out);
return out;
}
}; // class
} // namespace
#endif // TF2_ROS_BUFFER_INTERFACE_H

92
navigations/geometry2/tf2_ros/include/tf2_ros/buffer_server.h Executable file
View File

@@ -0,0 +1,92 @@
/*********************************************************************
*
* 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
*********************************************************************/
#ifndef TF2_ROS_BUFFER_SERVER_H_
#define TF2_ROS_BUFFER_SERVER_H_
#include <actionlib/server/action_server.h>
#include <tf2_msgs/LookupTransformAction.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/buffer.h>
namespace tf2_ros
{
/** \brief Action server for the actionlib-based implementation of tf2_ros::BufferInterface.
*
* Use this class with a tf2_ros::TransformListener in the same process.
* You can use this class with a tf2_ros::BufferClient in a different process.
*/
class BufferServer
{
private:
typedef actionlib::ActionServer<tf2_msgs::LookupTransformAction> LookupTransformServer;
typedef LookupTransformServer::GoalHandle GoalHandle;
struct GoalInfo
{
GoalHandle handle;
ros::Time end_time;
};
public:
/** \brief Constructor
* \param buffer The Buffer that this BufferServer will wrap.
* \param ns The namespace in which to look for action clients.
* \param auto_start Pass argument to the constructor of the ActionServer.
* \param check_period How often to check for changes to known transforms (via a timer event).
*/
BufferServer(const Buffer& buffer, const std::string& ns,
bool auto_start = true, ros::Duration check_period = ros::Duration(0.01));
/** \brief Start the action server.
*/
void start();
private:
void goalCB(GoalHandle gh);
void cancelCB(GoalHandle gh);
void checkTransforms(const ros::TimerEvent& e);
bool canTransform(GoalHandle gh);
geometry_msgs::TransformStamped lookupTransform(GoalHandle gh);
const Buffer& buffer_;
LookupTransformServer server_;
std::list<GoalInfo> active_goals_;
boost::mutex mutex_;
ros::Timer check_timer_;
};
}
#endif

716
navigations/geometry2/tf2_ros/include/tf2_ros/message_filter.h Executable file
View File

@@ -0,0 +1,716 @@
/*
* Copyright (c) 2010, 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 Josh Faust */
#ifndef TF2_ROS_MESSAGE_FILTER_H
#define TF2_ROS_MESSAGE_FILTER_H
#include <tf2/buffer_core.h>
#include <string>
#include <list>
#include <vector>
#include <boost/function.hpp>
#include <boost/bind/bind.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <message_filters/connection.h>
#include <message_filters/simple_filter.h>
#include <ros/node_handle.h>
#include <ros/callback_queue_interface.h>
#include <ros/init.h>
#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \
ROS_DEBUG_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__)
#define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \
ROS_WARN_NAMED("message_filter", std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), getTargetFramesString().c_str(), __VA_ARGS__)
namespace tf2_ros
{
namespace filter_failure_reasons
{
enum FilterFailureReason
{
/// The message buffer overflowed, and this message was pushed off the back of the queue, but the reason it was unable to be transformed is unknown.
Unknown,
/// The timestamp on the message is more than the cache length earlier than the newest data in the transform cache
OutTheBack,
/// The frame_id on the message is empty
EmptyFrameID,
};
}
typedef filter_failure_reasons::FilterFailureReason FilterFailureReason;
class MessageFilterBase
{
public:
typedef std::vector<std::string> V_string;
virtual ~MessageFilterBase(){}
virtual void clear() = 0;
virtual void setTargetFrame(const std::string& target_frame) = 0;
virtual void setTargetFrames(const V_string& target_frames) = 0;
virtual void setTolerance(const ros::Duration& tolerance) = 0;
};
/**
* \brief Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available
*
* The callbacks used in this class are of the same form as those used by roscpp's message callbacks.
*
* MessageFilter is templated on a message type.
*
* \section example_usage Example Usage
*
* If you want to hook a MessageFilter into a ROS topic:
\verbatim
message_filters::Subscriber<MessageType> sub(node_handle_, "topic", 10);
tf2_ros::MessageFilter<MessageType> tf_filter(sub, tf_buffer_, "/map", 10, 0);
tf_filter.registerCallback(&MyClass::myCallback, this);
\endverbatim
*/
template<class M>
class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter<M>
{
public:
typedef boost::shared_ptr<M const> MConstPtr;
typedef ros::MessageEvent<M const> MEvent;
typedef boost::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback;
typedef boost::signals2::signal<void(const MConstPtr&, FilterFailureReason)> FailureSignal;
// If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it
// Actually, we need to check that the message has a header, or that it
// has the FrameId and Stamp traits. However I don't know how to do that
// so simply commenting out for now.
//ROS_STATIC_ASSERT(ros::message_traits::HasHeader<M>::value);
/**
* \brief Constructor
*
* \param bc The tf2::BufferCore this filter should use
* \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function.
* \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous).
* \param nh The NodeHandle whose callback queue we should add callbacks to
*/
MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
: bc_(bc)
, queue_size_(queue_size)
, callback_queue_(nh.getCallbackQueue())
{
init();
setTargetFrame(target_frame);
}
/**
* \brief Constructor
*
* \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber.
* \param bc The tf2::BufferCore this filter should use
* \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function.
* \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous).
* \param nh The NodeHandle whose callback queue we should add callbacks to
*/
template<class F>
MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, const ros::NodeHandle& nh)
: bc_(bc)
, queue_size_(queue_size)
, callback_queue_(nh.getCallbackQueue())
{
init();
setTargetFrame(target_frame);
connectInput(f);
}
/**
* \brief Constructor
*
* \param bc The tf2::BufferCore this filter should use
* \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function.
* \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous).
* \param cbqueue The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either
* a) add() is called, which will generally be when the previous filter in the chain outputs a message, or
* b) tf2::BufferCore::setTransform() is called
*/
MessageFilter(tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue)
: bc_(bc)
, queue_size_(queue_size)
, callback_queue_(cbqueue)
{
init();
setTargetFrame(target_frame);
}
/**
* \brief Constructor
*
* \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber.
* \param bc The tf2::BufferCore this filter should use
* \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function.
* \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous).
* \param cbqueue The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either
* a) add() is called, which will generally be when the previous filter in the chain outputs a message, or
* b) tf2::BufferCore::setTransform() is called
*/
template<class F>
MessageFilter(F& f, tf2::BufferCore& bc, const std::string& target_frame, uint32_t queue_size, ros::CallbackQueueInterface* cbqueue)
: bc_(bc)
, queue_size_(queue_size)
, callback_queue_(cbqueue)
{
init();
setTargetFrame(target_frame);
connectInput(f);
}
/**
* \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first.
*/
template<class F>
void connectInput(F& f)
{
message_connection_.disconnect();
message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this);
}
/**
* \brief Destructor
*/
~MessageFilter()
{
message_connection_.disconnect();
MessageFilter::clear();
TF2_ROS_MESSAGEFILTER_DEBUG("Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
(long long unsigned int)successful_transform_count_,
(long long unsigned int)failed_out_the_back_count_, (long long unsigned int)transform_message_count_,
(long long unsigned int)incoming_message_count_, (long long unsigned int)dropped_message_count_);
}
/**
* \brief Set the frame you need to be able to transform to before getting a message callback
*/
void setTargetFrame(const std::string& target_frame)
{
V_string frames;
frames.push_back(target_frame);
setTargetFrames(frames);
}
/**
* \brief Set the frames you need to be able to transform to before getting a message callback
*/
void setTargetFrames(const V_string& target_frames)
{
boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
target_frames_.resize(target_frames.size());
std::transform(target_frames.begin(), target_frames.end(), target_frames_.begin(), this->stripSlash);
expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2);
std::stringstream ss;
for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
{
ss << *it << " ";
}
target_frames_string_ = ss.str();
}
/**
* \brief Get the target frames as a string for debugging
*/
std::string getTargetFramesString()
{
boost::mutex::scoped_lock lock(target_frames_mutex_);
return target_frames_string_;
};
/**
* \brief Set the required tolerance for the notifier to return true
*/
void setTolerance(const ros::Duration& tolerance)
{
boost::mutex::scoped_lock lock(target_frames_mutex_);
time_tolerance_ = tolerance;
expected_success_count_ = target_frames_.size() * (time_tolerance_.isZero() ? 1 : 2);
}
/**
* \brief Clear any messages currently in the queue
*/
void clear()
{
boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared");
bc_.removeTransformableCallback(callback_handle_);
callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
messages_.clear();
message_count_ = 0;
// remove pending callbacks in callback queue as well
if (callback_queue_)
callback_queue_->removeByID((uint64_t)this);
warned_about_empty_frame_id_ = false;
}
void add(const MEvent& evt)
{
if (target_frames_.empty())
{
return;
}
namespace mt = ros::message_traits;
const MConstPtr& message = evt.getMessage();
std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
ros::Time stamp = mt::TimeStamp<M>::value(*message);
if (frame_id.empty())
{
messageDropped(evt, filter_failure_reasons::EmptyFrameID);
return;
}
// iterate through the target frames and add requests for each of them
MessageInfo info;
info.handles.reserve(expected_success_count_);
{
V_string target_frames_copy;
// Copy target_frames_ to avoid deadlock from #79
{
boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
target_frames_copy = target_frames_;
}
V_string::iterator it = target_frames_copy.begin();
V_string::iterator end = target_frames_copy.end();
for (; it != end; ++it)
{
const std::string& target_frame = *it;
tf2::TransformableRequestHandle handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp);
if (handle == 0xffffffffffffffffULL) // never transformable
{
messageDropped(evt, filter_failure_reasons::OutTheBack);
return;
}
else if (handle == 0)
{
++info.success_count;
}
else
{
info.handles.push_back(handle);
}
if (!time_tolerance_.isZero())
{
handle = bc_.addTransformableRequest(callback_handle_, target_frame, frame_id, stamp + time_tolerance_);
if (handle == 0xffffffffffffffffULL) // never transformable
{
messageDropped(evt, filter_failure_reasons::OutTheBack);
return;
}
else if (handle == 0)
{
++info.success_count;
}
else
{
info.handles.push_back(handle);
}
}
}
}
// We can transform already
if (info.success_count == expected_success_count_)
{
messageReady(evt);
}
else
{
boost::unique_lock< boost::shared_mutex > unique_lock(messages_mutex_);
// If this message is about to push us past our queue size, erase the oldest message
if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
{
++dropped_message_count_;
const MessageInfo& front = messages_.front();
TF2_ROS_MESSAGEFILTER_DEBUG("Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)", message_count_,
(mt::FrameId<M>::value(*front.event.getMessage())).c_str(), mt::TimeStamp<M>::value(*front.event.getMessage()).toSec());
V_TransformableRequestHandle::const_iterator it = front.handles.begin();
V_TransformableRequestHandle::const_iterator end = front.handles.end();
for (; it != end; ++it)
{
bc_.cancelTransformableRequest(*it);
}
messageDropped(front.event, filter_failure_reasons::Unknown);
messages_.pop_front();
--message_count_;
}
// Add the message to our list
info.event = evt;
messages_.push_back(info);
++message_count_;
}
TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_);
++incoming_message_count_;
}
/**
* \brief Manually add a message into this filter.
* \note If the message (or any other messages in the queue) are immediately transformable this will immediately call through to the output callback, possibly
* multiple times
*/
void add(const MConstPtr& message)
{
boost::shared_ptr<std::map<std::string, std::string> > header(new std::map<std::string, std::string>);
(*header)["callerid"] = "unknown";
ros::WallTime n = ros::WallTime::now();
ros::Time t(n.sec, n.nsec);
add(MEvent(message, header, t));
}
/**
* \brief Register a callback to be called when a message is about to be dropped
* \param callback The callback to call
*/
message_filters::Connection registerFailureCallback(const FailureCallback& callback)
{
boost::mutex::scoped_lock lock(failure_signal_mutex_);
return message_filters::Connection(boost::bind(&MessageFilter::disconnectFailure, this, boost::placeholders::_1), failure_signal_.connect(callback));
}
virtual void setQueueSize( uint32_t new_queue_size )
{
queue_size_ = new_queue_size;
}
virtual uint32_t getQueueSize()
{
return queue_size_;
}
private:
void init()
{
message_count_ = 0;
successful_transform_count_ = 0;
failed_out_the_back_count_ = 0;
transform_message_count_ = 0;
incoming_message_count_ = 0;
dropped_message_count_ = 0;
time_tolerance_ = ros::Duration(0.0);
warned_about_empty_frame_id_ = false;
expected_success_count_ = 1;
callback_handle_ = bc_.addTransformableCallback(boost::bind(&MessageFilter::transformable, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4, boost::placeholders::_5));
}
void transformable(tf2::TransformableRequestHandle request_handle, const std::string& /* target_frame */, const std::string& /* source_frame */,
ros::Time /* time */, tf2::TransformableResult result)
{
namespace mt = ros::message_traits;
boost::upgrade_lock< boost::shared_mutex > lock(messages_mutex_);
// find the message this request is associated with
typename L_MessageInfo::iterator msg_it = messages_.begin();
typename L_MessageInfo::iterator msg_end = messages_.end();
for (; msg_it != msg_end; ++msg_it)
{
MessageInfo& info = *msg_it;
V_TransformableRequestHandle::const_iterator handle_it = std::find(info.handles.begin(), info.handles.end(), request_handle);
if (handle_it != info.handles.end())
{
// found msg_it
++info.success_count;
break;
}
}
if (msg_it == msg_end)
{
return;
}
const MessageInfo& info = *msg_it;
if (info.success_count < expected_success_count_)
{
return;
}
bool can_transform = true;
const MConstPtr& message = info.event.getMessage();
std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
ros::Time stamp = mt::TimeStamp<M>::value(*message);
if (result == tf2::TransformAvailable)
{
boost::mutex::scoped_lock frames_lock(target_frames_mutex_);
// make sure we can still perform all the necessary transforms
typename V_string::iterator it = target_frames_.begin();
typename V_string::iterator end = target_frames_.end();
for (; it != end; ++it)
{
const std::string& target = *it;
if (!bc_.canTransform(target, frame_id, stamp))
{
can_transform = false;
break;
}
if (!time_tolerance_.isZero())
{
if (!bc_.canTransform(target, frame_id, stamp + time_tolerance_))
{
can_transform = false;
break;
}
}
}
}
else
{
can_transform = false;
}
// We will be mutating messages now, require unique lock
boost::upgrade_to_unique_lock< boost::shared_mutex > uniqueLock(lock);
if (can_transform)
{
TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1);
++successful_transform_count_;
messageReady(info.event);
}
else
{
++dropped_message_count_;
TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d", frame_id.c_str(), stamp.toSec(), message_count_ - 1);
messageDropped(info.event, filter_failure_reasons::Unknown);
}
messages_.erase(msg_it);
--message_count_;
}
/**
* \brief Callback that happens when we receive a message on the message topic
*/
void incomingMessage(const ros::MessageEvent<M const>& evt)
{
add(evt);
}
void checkFailures()
{
if (next_failure_warning_.isZero())
{
next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(15);
}
if (ros::WallTime::now() >= next_failure_warning_)
{
if (incoming_message_count_ - message_count_ == 0)
{
return;
}
double dropped_pct = (double)dropped_message_count_ / (double)(incoming_message_count_ - message_count_);
if (dropped_pct > 0.95)
{
TF2_ROS_MESSAGEFILTER_WARN("Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct*100, ROSCONSOLE_DEFAULT_NAME);
next_failure_warning_ = ros::WallTime::now() + ros::WallDuration(60);
if ((double)failed_out_the_back_count_ / (double)dropped_message_count_ > 0.5)
{
TF2_ROS_MESSAGEFILTER_WARN(" The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s", last_out_the_back_stamp_.toSec(), last_out_the_back_frame_.c_str());
}
}
}
}
struct CBQueueCallback : public ros::CallbackInterface
{
CBQueueCallback(MessageFilter* filter, const MEvent& event, bool success, FilterFailureReason reason)
: event_(event)
, filter_(filter)
, reason_(reason)
, success_(success)
{}
virtual CallResult call()
{
if (success_)
{
filter_->signalMessage(event_);
}
else
{
filter_->signalFailure(event_, reason_);
}
return Success;
}
private:
MEvent event_;
MessageFilter* filter_;
FilterFailureReason reason_;
bool success_;
};
void messageDropped(const MEvent& evt, FilterFailureReason reason)
{
if (callback_queue_)
{
ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason));
callback_queue_->addCallback(cb, (uint64_t)this);
}
else
{
signalFailure(evt, reason);
}
}
void messageReady(const MEvent& evt)
{
if (callback_queue_)
{
ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, filter_failure_reasons::Unknown));
callback_queue_->addCallback(cb, (uint64_t)this);
}
else
{
this->signalMessage(evt);
}
}
void disconnectFailure(const message_filters::Connection& c)
{
boost::mutex::scoped_lock lock(failure_signal_mutex_);
c.getBoostConnection().disconnect();
}
void signalFailure(const MEvent& evt, FilterFailureReason reason)
{
boost::mutex::scoped_lock lock(failure_signal_mutex_);
failure_signal_(evt.getMessage(), reason);
}
static
std::string stripSlash(const std::string& in)
{
if ( !in.empty() && (in[0] == '/'))
{
std::string out = in;
out.erase(0, 1);
return out;
}
return in;
}
tf2::BufferCore& bc_; ///< The Transformer used to determine if transformation data is available
V_string target_frames_; ///< The frames we need to be able to transform to before a message is ready
std::string target_frames_string_;
boost::mutex target_frames_mutex_; ///< A mutex to protect access to the target_frames_ list and target_frames_string.
uint32_t queue_size_; ///< The maximum number of messages we queue up
tf2::TransformableCallbackHandle callback_handle_;
typedef std::vector<tf2::TransformableRequestHandle> V_TransformableRequestHandle;
struct MessageInfo
{
MessageInfo()
: success_count(0)
{}
MEvent event;
V_TransformableRequestHandle handles;
uint32_t success_count;
};
typedef std::list<MessageInfo> L_MessageInfo;
L_MessageInfo messages_;
uint32_t message_count_; ///< The number of messages in the list. Used because \<container\>.size() may have linear cost
boost::shared_mutex messages_mutex_; ///< The mutex used for locking message list operations
uint32_t expected_success_count_;
bool warned_about_empty_frame_id_;
uint64_t successful_transform_count_;
uint64_t failed_out_the_back_count_;
uint64_t transform_message_count_;
uint64_t incoming_message_count_;
uint64_t dropped_message_count_;
ros::Time last_out_the_back_stamp_;
std::string last_out_the_back_frame_;
ros::WallTime next_failure_warning_;
ros::Duration time_tolerance_; ///< Provide additional tolerance on time for messages which are stamped but can have associated duration
message_filters::Connection message_connection_;
FailureSignal failure_signal_;
boost::mutex failure_signal_mutex_;
ros::CallbackQueueInterface* callback_queue_;
};
} // namespace tf2
#endif

75
navigations/geometry2/tf2_ros/include/tf2_ros/static_transform_broadcaster.h Executable file
View File

@@ -0,0 +1,75 @@
/*
* 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 */
#ifndef TF2_ROS_STATICTRANSFORMBROADCASTER_H
#define TF2_ROS_STATICTRANSFORMBROADCASTER_H
#include "ros/ros.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2_msgs/TFMessage.h"
namespace tf2_ros
{
/** \brief This class provides an easy way to publish coordinate frame transform information.
* It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the
* necessary data needed for each message. */
class StaticTransformBroadcaster{
public:
/** \brief Constructor (needs a ros::Node reference) */
StaticTransformBroadcaster();
/** \brief Send a TransformStamped message
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const geometry_msgs::TransformStamped & transform) {
sendTransform(std::vector<geometry_msgs::TransformStamped>({transform}));
}
/** \brief Send a vector of TransformStamped messages
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
private:
/// Internal reference to ros::Node
ros::NodeHandle node_;
ros::Publisher publisher_;
tf2_msgs::TFMessage net_message_;
};
}
#endif //TF_STATICTRANSFORMBROADCASTER_H

78
navigations/geometry2/tf2_ros/include/tf2_ros/transform_broadcaster.h Executable file
View File

@@ -0,0 +1,78 @@
/*
* 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 */
#ifndef TF2_ROS_TRANSFORMBROADCASTER_H
#define TF2_ROS_TRANSFORMBROADCASTER_H
#include "ros/ros.h"
#include "geometry_msgs/TransformStamped.h"
namespace tf2_ros
{
/** \brief This class provides an easy way to publish coordinate frame transform information.
* It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the
* necessary data needed for each message. */
class TransformBroadcaster{
public:
/** \brief Constructor (needs a ros::Node reference) */
TransformBroadcaster();
/** \brief Send a StampedTransform
* The stamped data structure includes frame_id, and time, and parent_id already. */
// void sendTransform(const StampedTransform & transform);
/** \brief Send a vector of StampedTransforms
* The stamped data structure includes frame_id, and time, and parent_id already. */
//void sendTransform(const std::vector<StampedTransform> & transforms);
/** \brief Send a TransformStamped message
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const geometry_msgs::TransformStamped & transform);
/** \brief Send a vector of TransformStamped messages
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
private:
/// Internal reference to ros::Node
ros::NodeHandle node_;
ros::Publisher publisher_;
};
}
#endif //TF_TRANSFORMBROADCASTER_H

92
navigations/geometry2/tf2_ros/include/tf2_ros/transform_listener.h Executable file
View File

@@ -0,0 +1,92 @@
/*
* 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 */
#ifndef TF2_ROS_TRANSFORMLISTENER_H
#define TF2_ROS_TRANSFORMLISTENER_H
#include "std_msgs/Empty.h"
#include "tf2_msgs/TFMessage.h"
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "tf2_ros/buffer.h"
#include "boost/thread.hpp"
namespace tf2_ros{
/** \brief This class provides an easy way to request and receive coordinate frame transform information.
*/
class TransformListener
{
public:
/**@brief Constructor for transform listener */
TransformListener(tf2::BufferCore& buffer, bool spin_thread = true,
ros::TransportHints transport_hints = ros::TransportHints());
TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread = true,
ros::TransportHints transport_hints = ros::TransportHints());
~TransformListener();
private:
/// Initialize this transform listener, subscribing, advertising services, etc.
void init();
void initWithThread();
/// Callback function for ros message subscription
void subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt);
void static_subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt);
void subscription_callback_impl(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt, bool is_static);
ros::CallbackQueue tf_message_callback_queue_;
boost::thread* dedicated_listener_thread_;
ros::NodeHandle node_;
ros::Subscriber message_subscriber_tf_;
ros::Subscriber message_subscriber_tf_static_;
tf2::BufferCore& buffer_;
bool using_dedicated_thread_;
ros::TransportHints transport_hints_;
ros::Time last_update_;
void dedicatedListenerThread()
{
while (using_dedicated_thread_)
{
tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
}
};
};
}
#endif //TF_TRANSFORMLISTENER_H