laser.cpp
Go to the documentation of this file.
1 /******************************************************************************
2  STDR Simulator - Simple Two DImensional Robot Simulator
3  Copyright (C) 2013 STDR Simulator
4  This program is free software; you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation; either version 3 of the License, or
7  (at your option) any later version.
8  This program is distributed in the hope that it will be useful,
9  but WITHOUT ANY WARRANTY; without even the implied warranty of
10  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  GNU General Public License for more details.
12  You should have received a copy of the GNU General Public License
13  along with this program; if not, write to the Free Software Foundation,
14  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
15 
16  Authors :
17  * Manos Tsardoulias, etsardou@gmail.com
18  * Aris Thallas, aris.thallas@gmail.com
19  * Chris Zalidis, zalidis@gmail.com
20 ******************************************************************************/
21 
23 
24 namespace stdr_robot {
25 
34  Laser::Laser(const nav_msgs::OccupancyGrid& map,
35  const stdr_msgs::LaserSensorMsg& msg,
36  const std::string& name,
37  ros::NodeHandle& n)
38  :
39  Sensor(map, name, n, msg.pose, msg.frame_id, msg.frequency)
40  {
41  _description = msg;
42 
43  _publisher = n.advertise<sensor_msgs::LaserScan>
44  ( _namespace + "/" + msg.frame_id, 1 );
45  }
46 
52  {
53  float angle;
54  int distance;
55  int xMap, yMap;
56  int divisions = 1;
57  sensor_msgs::LaserScan _laserScan;
58 
59  if ( _description.numRays > 1 )
60  {
61  divisions = _description.numRays - 1;
62  }
63 
64  _laserScan.angle_min = _description.minAngle;
65  _laserScan.angle_max = _description.maxAngle;
66  _laserScan.range_max = _description.maxRange;
67  _laserScan.range_min = _description.minRange;
68  _laserScan.angle_increment =
69  ( _description.maxAngle - _description.minAngle ) / divisions;
70 
71 
72  if ( _map.info.height == 0 || _map.info.width == 0 )
73  {
74  ROS_DEBUG("Outside limits\n");
75  return;
76  }
77  for ( int laserScanIter = 0; laserScanIter < _description.numRays;
78  laserScanIter++ )
79  {
80 
82  _description.minAngle + laserScanIter *
83  ( _description.maxAngle - _description.minAngle )
84  / divisions;
85 
86  distance = 1;
87 
88  while ( distance <= _description.maxRange / _map.info.resolution )
89  {
90  xMap = _sensorTransform.getOrigin().x() / _map.info.resolution +
91  cos( angle ) * distance;
92 
93  yMap = _sensorTransform.getOrigin().y() / _map.info.resolution +
94  sin( angle ) * distance;
95 
96  if (yMap * _map.info.width + xMap > _map.info.height*_map.info.width ||
97  yMap * _map.info.width + xMap < 0)
98  {
99  distance = _description.maxRange / _map.info.resolution - 1;
100  break;
101  }
102 
103  if ( _map.data[ yMap * _map.info.width + xMap ] > 70 )
104  {
105  break;
106  }
107 
108  distance ++;
109  }
110 
111  if ( distance * _map.info.resolution > _description.maxRange )
112  _laserScan.ranges.push_back( std::numeric_limits<float>::infinity() );
113  else if ( distance * _map.info.resolution < _description.minRange )
114  _laserScan.ranges.push_back(- std::numeric_limits<float>::infinity() );
115  else
116  _laserScan.ranges.push_back( distance * _map.info.resolution );
117  }
118 
119  _laserScan.header.stamp = ros::Time::now();
120  _laserScan.header.frame_id = _namespace + "_" + _description.frame_id;
121  _publisher.publish( _laserScan );
122  }
123 
124 } // namespace stdr_robot
const std::string & _namespace
< The base for the sensor frame_id
Definition: sensor_base.h:114
void publish(const boost::shared_ptr< M > &message) const
The main namespace for STDR Robot.
Definition: exceptions.h:27
static double getYaw(const Quaternion &bt_q)
A class that provides sensor abstraction.
Definition: sensor_base.h:40
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual void updateSensorCallback()
Updates the sensor measurements.
Definition: laser.cpp:51
tf::StampedTransform _sensorTransform
True if sensor got the _sensorTransform.
Definition: sensor_base.h:135
TFSIMD_FORCE_INLINE const tfScalar & y() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
const nav_msgs::OccupancyGrid & _map
Sensor pose relative to robot.
Definition: sensor_base.h:116
Quaternion getRotation() const
Laser(const nav_msgs::OccupancyGrid &map, const stdr_msgs::LaserSensorMsg &msg, const std::string &name, ros::NodeHandle &n)
Default constructor.
Definition: laser.cpp:34
static Time now()
ros::Publisher _publisher
ROS tf listener.
Definition: sensor_base.h:131
#define ROS_DEBUG(...)
stdr_msgs::LaserSensorMsg _description
< Laser sensor description
Definition: laser.h:71


stdr_robot
Author(s): Manos Tsardoulias, Chris Zalidis, Aris Thallas
autogenerated on Mon Jun 10 2019 15:15:10