Fawkes API
Fawkes Development Version
|
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>
37 using namespace gazebo;
46 :
Thread(
"LaserSimThread",
Thread::OPMODE_WAITFORWAKEUP),
67 laser_sub_ =
gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg,
this);
70 laser_data_ = (
float *)malloc(
sizeof(
float) * 360);
100 LaserSimThread::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;
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
const char * name() const
virtual void init()
Initialize the thread.
void set_timestamp(const Time *t=NULL)
Set timestamp.
virtual void close(Interface *interface)=0
Time now() const
Get the current time.
void set_frame(const char *new_frame)
Set frame value.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
virtual float get_float(const char *path)=0
virtual std::string get_string(const char *path)=0
virtual void log_debug(const char *component, const char *format,...)=0
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
LaserSimThread()
Constructor.