22#include "gazsim_laser_thread.h"
24#include <aspect/logging.h>
25#include <core/threading/mutex_locker.h>
26#include <interfaces/Laser360Interface.h>
28#include <utils/math/angle.h>
32#include <gazebo/msgs/msgs.hh>
33#include <gazebo/transport/Node.hh>
34#include <gazebo/transport/transport.hh>
37using namespace gazebo;
67 laser_sub_ =
gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg,
this);
70 laser_data_ = (
float *)malloc(
sizeof(
float) * 360);
100LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
106 const gazebo::msgs::LaserScan &scan = msg->scan();
109 int start_index = (scan.angle_min() + 2 * M_PI) / M_PI * 180;
111 int number_beams = scan.ranges_size();
116 for (
int i = 0; i < number_beams; i++) {
117 const float range = scan.ranges(i);
118 if (range < max_range_) {
119 laser_data_[(start_index + i) % 360] = range;
121 laser_data_[(start_index + i) % 360] = NAN;
LaserSimThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Time now() const
Get the current time.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
void write()
Write from local copy into BlackBoard memory.
void set_timestamp(const Time *t=NULL)
Set timestamp.
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Thread class encapsulation of pthreads.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.