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 <tf/types.h>
25 #include <utils/math/angle.h>
26 #include <core/threading/mutex_locker.h>
27 
28 #include <interfaces/Laser360Interface.h>
29 
30 #include <gazebo/transport/Node.hh>
31 #include <gazebo/msgs/msgs.hh>
32 #include <gazebo/transport/transport.hh>
33 #include <aspect/logging.h>
34 
35 #include <cstdio>
36 #include <cmath>
37 
38 using namespace fawkes;
39 using namespace gazebo;
40 
41 /** @class LaserSimThread "gazsim_laser_thread.h"
42  * Thread simulates the Hokuyo in Gazebo
43  * @author Frederik Zwilling
44  */
45 
46 /** Constructor. */
48  : Thread("LaserSimThread", Thread::OPMODE_WAITFORWAKEUP),
49  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
50 {
51 }
52 
54 {
55  logger->log_debug(name(), "Initializing Simulation of the Laser Sensor");
56 
57  //read config values
58  max_range_ = config->get_float("/gazsim/laser/max_range");
59  laser_topic_ = config->get_string("/gazsim/topics/laser");
60  interface_id_ = config->get_string("/gazsim/laser/interface-id");
61  frame_id_ = config->get_string("/gazsim/laser/frame-id");
62 
63  //open interface
64  laser_if_ = blackboard->open_for_writing<Laser360Interface>(interface_id_.c_str());
65  laser_if_->set_auto_timestamping(false);
66 
67  //subscribing to gazebo publisher
68  laser_sub_ = gazebonode->Subscribe(laser_topic_, &LaserSimThread::on_laser_data_msg, this);
69 
70  //initialize laser data
71  laser_data_ = (float *)malloc(sizeof(float) * 360);
72  laser_time_ = new Time(clock);
73  new_data_ = false;
74 
75  //set frame in the interface
76  laser_if_->set_frame(frame_id_.c_str());
77 }
78 
80 {
81  blackboard->close(laser_if_);
82  free(laser_data_);
83  delete laser_time_;
84 }
85 
87 {
88  if(new_data_)
89  {
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 LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
100 {
101  //logger->log_info(name(), "Got new Laser data.\n");
102 
103  MutexLocker lock(loop_mutex);
104 
105  const gazebo::msgs::LaserScan &scan = msg->scan();
106 
107  //calculate start angle
108  int start_index = (scan.angle_min() + 2* M_PI) / M_PI * 180;
109 
110  int number_beams = scan.ranges_size();
111 
112  *laser_time_ = clock->now();
113 
114  //copy laser data
115  for(int i = 0; i < number_beams; i++)
116  {
117  const float range = scan.ranges(i);
118  if(range < max_range_)
119  {
120  laser_data_[(start_index + i) % 360] = range;
121  }
122  else
123  {
124  laser_data_[(start_index + i) % 360] = NAN;
125  }
126 
127  }
128  new_data_ = true;
129 }
Laser360Interface Fawkes BlackBoard Interface.
LaserSimThread()
Constructor.
void set_frame(const char *new_frame)
Set frame value.
Definition: gps.h:30
virtual void init()
Initialize the thread.
void set_auto_timestamping(bool enabled)
Enable or disable automated timestamping.
Definition: interface.cpp:760
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
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().
Definition: thread.h:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Time now() const
Get the current time.
Definition: clock.cpp:269
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.
Definition: thread.h:95
gazebo::transport::NodePtr gazebonode
Gazebo Node for communication with a robot.
Definition: gazebo.h:47
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.
Definition: configurable.h:44
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.
Definition: interface.cpp:729
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.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.