22 #include "gazsim_localization_thread.h"
24 #include <aspect/logging.h>
25 #include <core/threading/mutex_locker.h>
26 #include <interfaces/Position3DInterface.h>
27 #include <tf/transform_publisher.h>
29 #include <utils/math/angle.h>
30 #include <utils/time/clock.h>
32 #include <gazebo/msgs/msgs.hh>
33 #include <gazebo/transport/Node.hh>
34 #include <gazebo/transport/transport.hh>
39 using namespace gazebo;
48 :
Thread(
"LocalizationSimThread",
Thread::OPMODE_WAITFORWAKEUP),
64 transform_tolerance_ =
config->
get_float(
"/plugins/amcl/transform_tolerance");
70 gazebonode->Subscribe(gps_topic_, &LocalizationSimThread::on_localization_msg,
this);
86 localization_if_->
set_frame(global_frame_id_.c_str());
95 localization_if_->
write();
98 tf::Quaternion rotation = tf::Quaternion(quat_x_, quat_y_, quat_z_, quat_w_);
99 tf::Point position = tf::Point(x_, y_, z_);
100 tf::Transform latest_tf_ = tf::Transform(rotation, position);
101 Time transform_expiration = (
clock->
now() + transform_tolerance_);
103 transform_expiration,
112 LocalizationSimThread::on_localization_msg(ConstPosePtr &msg)
119 x_ = msg->position().x();
120 y_ = msg->position().y();
121 z_ = msg->position().z();
122 quat_x_ = msg->orientation().x();
123 quat_y_ = msg->orientation().y();
124 quat_z_ = msg->orientation().z();
125 quat_w_ = msg->orientation().w();