39 FramePair(
const std::string& source_frame,
const std::string& target_frame,
double translational_update_distance,
double angular_update_distance) :
60 bool getFramePairs(
const ros::NodeHandle& local_node, std::vector<FramePair>& frame_pairs,
double default_translational_update_distance,
double default_angular_update_distance)
63 if (!local_node.
getParam(
"frame_pairs", frame_pairs_param))
66 frame_pairs.push_back(
FramePair(
"base_link",
"map", default_translational_update_distance, default_angular_update_distance));
72 ROS_ERROR(
"Expecting a list for frame_pairs parameter");
75 for (
int i = 0; i < frame_pairs_param.
size(); i++)
80 ROS_ERROR(
"frame_pairs must be specified as maps, but they are XmlRpcType: %d", frame_pair_param.
getType());
85 if (!frame_pair_param.
hasMember(
"source_frame"))
87 ROS_ERROR(
"frame_pair does not specified source_frame");
93 ROS_ERROR(
"source_frame must be a string, but it is XmlRpcType: %d", source_frame_param.
getType());
96 std::string source_frame = source_frame_param;
99 if (!frame_pair_param.
hasMember(
"target_frame"))
101 ROS_ERROR(
"frame_pair does not specified target_frame");
107 ROS_ERROR(
"target_frame must be a string, but it is XmlRpcType: %d", target_frame_param.
getType());
110 std::string target_frame = target_frame_param;
113 double translational_update_distance = default_translational_update_distance;
114 if (frame_pair_param.
hasMember(
"translational_update_distance"))
116 XmlRpc::XmlRpcValue translational_update_distance_param = frame_pair_param[
"translational_update_distance"];
120 ROS_ERROR(
"translational_update_distance must be either an integer or a double, but it is XmlRpcType: %d", translational_update_distance_param.
getType());
123 translational_update_distance = translational_update_distance_param;
127 double angular_update_distance = default_angular_update_distance;
128 if (frame_pair_param.
hasMember(
"angular_update_distance"))
130 XmlRpc::XmlRpcValue angular_update_distance_param = frame_pair_param[
"angular_update_distance"];
134 ROS_ERROR(
"angular_update_distance must be either an integer or a double, but it is XmlRpcType: %d", angular_update_distance_param.
getType());
137 angular_update_distance = angular_update_distance_param;
140 ROS_INFO(
"Notifying change on %s -> %s (translational update distance: %.4f, angular update distance: %.4f)", source_frame.c_str(), target_frame.c_str(), translational_update_distance, angular_update_distance);
142 frame_pairs.push_back(
FramePair(source_frame, target_frame, translational_update_distance, angular_update_distance));
152 int main(
int argc,
char** argv)
158 double polling_frequency, translational_update_distance, angular_update_distance;
159 local_node.
param(std::string(
"polling_frequency"), polling_frequency, 10.0);
160 local_node.
param(std::string(
"translational_update_distance"), translational_update_distance, 0.10);
161 local_node.
param(std::string(
"angular_update_distance"), angular_update_distance, 0.10);
163 std::vector<FramePair> frame_pairs;
164 if (!
getFramePairs(local_node, frame_pairs, translational_update_distance, angular_update_distance))
178 for (std::vector<FramePair>::iterator i = frame_pairs.begin(); i != frame_pairs.end(); i++)
193 geometry_msgs::TransformStamped msgtf;
195 msg.transforms.push_back(msgtf);
199 if (msg.transforms.size() > 0)
208 if (polling_frequency > 0)
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
void publish(const boost::shared_ptr< M > &message) const
FramePair(const std::string &source_frame, const std::string &target_frame, double translational_update_distance, double angular_update_distance)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
tf::Stamped< tf::Pose > last_sent_pose_
tf::Stamped< tf::Pose > pose_in_
bool getFramePairs(const ros::NodeHandle &local_node, std::vector< FramePair > &frame_pairs, double default_translational_update_distance, double default_angular_update_distance)
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
Return the distance between the ends of this and another vector This is symantically treating the vec...
Type const & getType() const
static tf::Quaternion createIdentityQuaternion()
construct an Identity Quaternion
double translational_update_distance_
tfScalar angle(const Quaternion &q) const
Return the ***half*** angle between this quaternion and the other.
Duration & fromSec(double t)
int main(int argc, char **argv)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double angular_update_distance_
std::string source_frame_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string target_frame_
ros::Time stamp_
The timestamp associated with this data.
bool hasMember(const std::string &name) const
bool getParam(const std::string &key, std::string &s) const
static void transformStampedTFToMsg(const StampedTransform &bt, geometry_msgs::TransformStamped &msg)
convert tf::StampedTransform to TransformStamped msg
tf::Stamped< tf::Pose > pose_out_
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...