40 #include <boost/foreach.hpp> 56 BOOST_FOREACH (
const geometry_msgs::TransformStamped& trans, (*it)->transforms)
58 const geometry_msgs::Vector3& v = trans.transform.translation;
59 const geometry_msgs::Quaternion& q = trans.transform.rotation;
60 const std_msgs::Header& h = trans.header;
65 ROS_ASSERT_MSG(ok,
"Tf setTransform returned false for transform from %s to %s at %.4f",
66 trans.child_frame_id.c_str(), h.frame_id.c_str(), h.stamp.toSec());
QueryResults< M >::range_t query(Query::ConstPtr query, bool metadata_only=false, const std::string &sort_by="", bool ascending=true) const
std::pair< ResultIterator< M >, ResultIterator< M > > range_t
#define ROS_ASSERT_MSG(cond,...)
Query::Ptr createQuery() const