Fawkes API
Fawkes Development Version
|
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();
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_frame(const char *new_frame)
Set frame value.
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
Thread aspect to use blocked timing.
const char * name() const
Get name of thread.
Clock * clock
By means of this member access to the clock is given.
virtual void loop()
Code to execute in the thread.
LocalizationSimThread()
Constructor.
Logger * logger
This is the Logger member used to access the logger.
virtual void close(Interface *interface)=0
Close interface.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Time now() const
Get the current time.
Fawkes library namespace.
Configuration * config
This is the Configuration member used to access the configuration.
A class for handling time.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
Thread class encapsulation of pthreads.
Position3DInterface Fawkes BlackBoard Interface.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual void init()
Initialize the thread.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
virtual void finalize()
Finalize the thread.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void write()
Write from local copy into BlackBoard memory.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.