Fawkes API Fawkes Development Version
gazsim_laser_thread.cpp
1
2/***************************************************************************
3 * gazsim_laser_thread.cpp - Thread simulate the Hokuyo in Gazebo
4 *
5 * Created: Thu Aug 08 15:51:41 2013
6 * Copyright 2013 Frederik Zwilling
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "gazsim_laser_thread.h"
23
24#include <aspect/logging.h>
25#include <core/threading/mutex_locker.h>
26#include <interfaces/Laser360Interface.h>
27#include <tf/types.h>
28#include <utils/math/angle.h>
29
30#include <cmath>
31#include <cstdio>
32#include <gazebo/msgs/msgs.hh>
33#include <gazebo/transport/Node.hh>
34#include <gazebo/transport/transport.hh>
35
36using namespace fawkes;
37using namespace gazebo;
38
39/** @class LaserSimThread "gazsim_laser_thread.h"
40 * Thread simulates the Hokuyo in Gazebo
41 * @author Frederik Zwilling
42 */
43
44/** Constructor. */
46: Thread("LaserSimThread", Thread::OPMODE_WAITFORWAKEUP),
47 BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
48{
49}
50
51void
53{
54 logger->log_debug(name(), "Initializing Simulation of the Laser Sensor");
55
56 //read config values
57 max_range_ = config->get_float("/gazsim/laser/max_range");
58 laser_topic_ = config->get_string("/gazsim/topics/laser");
59 interface_id_ = config->get_string("/gazsim/laser/interface-id");
60 frame_id_ = config->get_string("/gazsim/laser/frame-id");
61
62 //open interface
63 laser_if_ = blackboard->open_for_writing<Laser360Interface>(interface_id_.c_str());
64 laser_if_->set_auto_timestamping(false);
65
66 logger->log_info(name(), "Subscribing to laser topic '%s'", laser_topic_.c_str());
67 laser_sub_ = gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg, this);
68
69 //initialize laser data
70 laser_data_ = (float *)malloc(sizeof(float) * 360);
71 laser_time_ = new Time(clock);
72 new_data_ = false;
73
74 //set frame in the interface
75 laser_if_->set_frame(frame_id_.c_str());
76}
77
78void
80{
81 blackboard->close(laser_if_);
82 free(laser_data_);
83 delete laser_time_;
84}
85
86void
88{
89 if (new_data_) {
90 //write interface
91 laser_if_->set_distances(laser_data_);
92 laser_if_->set_timestamp(laser_time_);
93 laser_if_->write();
94
95 new_data_ = false;
96 }
97}
98
99void
100LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
101{
102 //logger->log_info(name(), "Got new Laser data.\n");
103
105
106 const gazebo::msgs::LaserScan &scan = msg->scan();
107
108 //calculate start angle
109 int start_index = (scan.angle_min() + 2 * M_PI) / M_PI * 180;
110
111 int number_beams = scan.ranges_size();
112
113 *laser_time_ = clock->now();
114
115 //copy laser data
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;
120 } else {
121 laser_data_[(start_index + i) % 360] = NAN;
122 }
123 }
124 new_data_ = true;
125}
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.
Definition: blackboard.h:44
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.
Definition: clock.h:42
Time now() const
Get the current time.
Definition: clock.cpp:242
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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.
Definition: gazebo.h:46
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:755
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:724
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.
Definition: logging.h:41
Mutex locking helper.
Definition: mutex_locker.h:34
Thread class encapsulation of pthreads.
Definition: thread.h:46
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Fawkes library namespace.