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 
36 using namespace fawkes;
37 using 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 
51 void
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  //subscribing to gazebo publisher
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 
78 void
80 {
81  blackboard->close(laser_if_);
82  free(laser_data_);
83  delete laser_time_;
84 }
85 
86 void
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 
99 void
100 LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
101 {
102  //logger->log_info(name(), "Got new Laser data.\n");
103 
104  MutexLocker lock(loop_mutex);
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 }
fawkes::Laser360Interface::set_distances
void set_distances(unsigned int index, const float new_distances)
Set distances value at given index.
Definition: Laser360Interface.cpp:172
LaserSimThread::loop
virtual void loop()
Code to execute in the thread.
Definition: gazsim_laser_thread.cpp:87
fawkes::MutexLocker
Definition: mutex_locker.h:39
LaserSimThread::finalize
virtual void finalize()
Finalize the thread.
Definition: gazsim_laser_thread.cpp:79
fawkes::GazeboAspect::gazebonode
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
Definition: gazebo.h:52
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
fawkes::ClockAspect::clock
Clock * clock
Definition: clock.h:56
LaserSimThread::init
virtual void init()
Initialize the thread.
Definition: gazsim_laser_thread.cpp:52
fawkes::Interface::set_timestamp
void set_timestamp(const Time *t=NULL)
Set timestamp.
Definition: interface.cpp:717
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Clock::now
Time now() const
Get the current time.
Definition: clock.cpp:249
fawkes
fawkes::Laser360Interface::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: Laser360Interface.cpp:105
fawkes::ConfigurableAspect::config
Configuration * config
Definition: configurable.h:53
fawkes::Time
Definition: time.h:98
fawkes::Interface::set_auto_timestamping
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:748
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
fawkes::Thread
Definition: thread.h:45
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Thread::loop_mutex
Mutex * loop_mutex
Definition: thread.h:152
fawkes::Laser360Interface
Definition: Laser360Interface.h:39
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:499
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
LaserSimThread::LaserSimThread
LaserSimThread()
Constructor.
Definition: gazsim_laser_thread.cpp:45