52 #include <core/macros.h>
53 #include <core/threading/mutex_locker.h>
54 #include <tf/exceptions.h>
55 #include <tf/time_cache.h>
56 #include <tf/transformer.h>
170 return (
frameIDs_.count(frame_id_str) > 0);
177 TimeCacheInterfacePtr
186 std::vector<TimeCacheInterfacePtr>
195 std::vector<std::string>
211 const std::string & source_frame,
213 std::string * error_msg)
const
215 if (likely(enabled_)) {
216 std::string stripped_target = strip_slash(target_frame);
217 std::string stripped_source = strip_slash(source_frame);
237 const std::string & source_frame,
239 const std::string & fixed_frame,
240 std::string * error_msg)
const
242 if (likely(enabled_)) {
243 std::string stripped_target = strip_slash(target_frame);
244 std::string stripped_source = strip_slash(source_frame);
246 stripped_target, target_time, stripped_source, source_time, fixed_frame, error_msg);
266 const std::string & source_frame,
268 StampedTransform & transform)
const
271 throw DisabledException(
"Transformer has been disabled");
274 std::string stripped_target = strip_slash(target_frame);
275 std::string stripped_source = strip_slash(source_frame);
301 const std::string & source_frame,
303 const std::string & fixed_frame,
309 std::string stripped_target = strip_slash(target_frame);
310 std::string stripped_source = strip_slash(source_frame);
313 stripped_target, target_time, stripped_source, source_time, fixed_frame, transform);
329 const std::string &source_frame,
355 assert_quaternion_valid(stamped_in);
360 stamped_out.
set_data(transform * stamped_in);
362 stamped_out.
frame_id = target_frame;
387 Vector3 end = stamped_in;
388 Vector3 origin = Vector3(0, 0, 0);
389 Vector3 output = (transform * end) - (transform * origin);
393 stamped_out.
frame_id = target_frame;
417 stamped_out.
set_data(transform * stamped_in);
419 stamped_out.
frame_id = target_frame;
443 stamped_out.
set_data(transform * stamped_in);
445 stamped_out.
frame_id = target_frame;
469 const std::string &target_frame,
506 const Stamped<Quaternion> &stamped_in,
507 const std::string & fixed_frame,
508 Stamped<Quaternion> & stamped_out)
const
510 assert_quaternion_valid(stamped_in);
511 StampedTransform transform;
513 target_frame, target_time, stamped_in.frame_id, stamped_in.
stamp, fixed_frame, transform);
515 stamped_out.set_data(transform * stamped_in);
516 stamped_out.stamp = transform.stamp;
517 stamped_out.frame_id = target_frame;
543 const std::string & fixed_frame,
548 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
551 Vector3 end = stamped_in;
552 Vector3 origin(0, 0, 0);
553 Vector3 output = (transform * end) - (transform * origin);
557 stamped_out.
frame_id = target_frame;
583 const std::string & fixed_frame,
588 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
592 stamped_out.
frame_id = target_frame;
618 const std::string & fixed_frame,
623 target_frame, target_time, stamped_in.
frame_id, stamped_in.
stamp, fixed_frame, transform);
627 stamped_out.
frame_id = target_frame;
642 *time = current_time;
644 std::stringstream mstream;
645 mstream << std::fixed;
646 mstream.precision(3);
647 mstream <<
"digraph { graph [fontsize=14";
649 mstream <<
", label=\"\\nRecorded at time: " << current_time.
str() <<
" ("
650 << current_time.
in_sec() <<
")\"";
652 mstream <<
"]; node [fontsize=12]; edge [fontsize=12]; " << std::endl;
657 mstream <<
"\"no tf data received\"";
659 mstream.precision(3);
660 mstream.setf(std::ios::fixed, std::ios::floatfield);
663 for (
unsigned int cnt = 1; cnt <
frames_.size(); ++cnt)
665 std::shared_ptr<TimeCacheInterface> cache =
get_frame(cnt);
669 unsigned int frame_id_num;
675 if (frame_id_num != 0) {
676 std::string authority =
"no recorded authority";
677 std::map<unsigned int, std::string>::const_iterator it =
frame_authority_.find(cnt);
679 authority = it->second;
681 double rate = cache->get_list_length()
682 / std::max((cache->get_latest_timestamp().in_sec()
683 - cache->get_oldest_timestamp().in_sec()),
691 std::shared_ptr<StaticCache> static_cache = std::dynamic_pointer_cast<StaticCache>(cache);
696 mstream <<
"Average rate: " << rate <<
" Hz\\n"
697 <<
"Most recent transform: "
698 << (current_time - cache->get_latest_timestamp()).in_sec() <<
" sec old \\n"
700 << (cache->get_latest_timestamp() - cache->get_oldest_timestamp()).in_sec()
703 mstream <<
"\"];" << std::endl;
708 return mstream.str();