35 const stdr_msgs::LaserSensorMsg& msg,
36 const std::string& name,
39 Sensor(map, name, n, msg.pose, msg.frame_id, msg.frequency)
57 sensor_msgs::LaserScan _laserScan;
68 _laserScan.angle_increment =
72 if (
_map.info.height == 0 ||
_map.info.width == 0 )
77 for (
int laserScanIter = 0; laserScanIter <
_description.numRays;
91 cos( angle ) * distance;
94 sin( angle ) * distance;
96 if (yMap *
_map.info.width + xMap >
_map.info.height*
_map.info.width ||
97 yMap *
_map.info.width + xMap < 0)
103 if (
_map.data[ yMap *
_map.info.width + xMap ] > 70 )
112 _laserScan.ranges.push_back( std::numeric_limits<float>::infinity() );
114 _laserScan.ranges.push_back(- std::numeric_limits<float>::infinity() );
116 _laserScan.ranges.push_back( distance *
_map.info.resolution );
const std::string & _namespace
< The base for the sensor frame_id
void publish(const boost::shared_ptr< M > &message) const
The main namespace for STDR Robot.
static double getYaw(const Quaternion &bt_q)
A class that provides sensor abstraction.
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.
tf::StampedTransform _sensorTransform
True if sensor got the _sensorTransform.
TFSIMD_FORCE_INLINE const tfScalar & y() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const nav_msgs::OccupancyGrid & _map
Sensor pose relative to robot.
Laser(const nav_msgs::OccupancyGrid &map, const stdr_msgs::LaserSensorMsg &msg, const std::string &name, ros::NodeHandle &n)
Default constructor.
ros::Publisher _publisher
ROS tf listener.
stdr_msgs::LaserSensorMsg _description
< Laser sensor description