22 #include "gazsim_localization_thread.h" 27 #include <utils/math/angle.h> 28 #include <core/threading/mutex_locker.h> 30 #include <interfaces/Position3DInterface.h> 32 #include <gazebo/transport/Node.hh> 33 #include <gazebo/msgs/msgs.hh> 34 #include <gazebo/transport/transport.hh> 35 #include <aspect/logging.h> 36 #include <tf/transform_publisher.h> 37 #include <utils/time/clock.h> 49 :
Thread(
"LocalizationSimThread",
Thread::OPMODE_WAITFORWAKEUP),
64 transform_tolerance_ =
config->
get_float(
"/plugins/amcl/transform_tolerance");
69 localization_sub_ =
gazebonode->Subscribe(gps_topic_, &LocalizationSimThread::on_localization_msg,
this);
84 localization_if_->
set_frame(global_frame_id_.c_str());
93 localization_if_->
write();
96 tf::Quaternion rotation = tf::Quaternion(quat_x_, quat_y_, quat_z_, quat_w_);
97 tf::Point position = tf::Point(x_, y_, z_);
98 tf::Transform latest_tf_ = tf::Transform(rotation,position);
99 Time transform_expiration =
100 (
clock->
now() + transform_tolerance_);
102 global_frame_id_, odom_frame_id_);
108 void LocalizationSimThread::on_localization_msg(ConstPosePtr &msg)
115 x_ = msg->position().x();
116 y_ = msg->position().y();
117 z_ = msg->position().z();
118 quat_x_ = msg->orientation().x();
119 quat_y_ = msg->orientation().y();
120 quat_z_ = msg->orientation().z();
121 quat_w_ = msg->orientation().w();
void set_frame(const char *new_frame)
Set frame value.
Fawkes library namespace.
A class for handling time.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Logger * logger
This is the Logger member used to access the logger.
Clock * clock
By means of this member access to the clock is given.
Time now() const
Get the current time.
Thread aspect to use blocked timing.
Position3DInterface Fawkes BlackBoard Interface.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void loop()
Code to execute in the thread.
Configuration * config
This is the Configuration member used to access the configuration.
LocalizationSimThread()
Constructor.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual void finalize()
Finalize the thread.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.