22 #include "gazsim_laser_thread.h" 25 #include <utils/math/angle.h> 26 #include <core/threading/mutex_locker.h> 28 #include <interfaces/Laser360Interface.h> 30 #include <gazebo/transport/Node.hh> 31 #include <gazebo/msgs/msgs.hh> 32 #include <gazebo/transport/transport.hh> 33 #include <aspect/logging.h> 48 :
Thread(
"LaserSimThread",
Thread::OPMODE_WAITFORWAKEUP),
68 laser_sub_ =
gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg,
this);
71 laser_data_ = (
float *)malloc(
sizeof(
float) * 360);
99 void LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
105 const gazebo::msgs::LaserScan &scan = msg->scan();
108 int start_index = (scan.angle_min() + 2* M_PI) / M_PI * 180;
110 int number_beams = scan.ranges_size();
115 for(
int i = 0; i < number_beams; i++)
117 const float range = scan.ranges(i);
118 if(range < max_range_)
120 laser_data_[(start_index + i) % 360] = range;
124 laser_data_[(start_index + i) % 360] = NAN;
Laser360Interface Fawkes BlackBoard Interface.
LaserSimThread()
Constructor.
void set_frame(const char *new_frame)
Set frame value.
virtual void init()
Initialize the thread.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Fawkes library namespace.
A class for handling time.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
void set_distances(unsigned int index, const float new_distances)
Set distances 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.
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in 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.
Configuration * config
This is the Configuration member used to access the configuration.
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.
void set_timestamp(const Time *t=NULL)
Set timestamp.
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.