Exemplos de ConstLaserScanStampedPtr em C++ (Cpp)

Exemplo n.º 1

Arquivo: gazsim_laser_thread.cpp Projeto: tempbottle/fawkes

void LaserSimThread::on_laser_data_msg(ConstLaserScanStampedPtr &msg)
{
  //logger->log_info(name(), "Got new Laser data.\n");

  MutexLocker lock(loop_mutex);

  const gazebo::msgs::LaserScan &scan = msg->scan();

  //calculate start angle
  int start_index = (scan.angle_min() + 2* M_PI) / M_PI * 180;
  
  int number_beams = scan.ranges_size();

  *laser_time_ = clock->now();
  
  //copy laser data
  for(int i = 0; i < number_beams; i++)
  {
    const float range = scan.ranges(i);
    if(range < max_range_)
    {
      laser_data_[(start_index + i) % 360] = range; 
    }
    else
    {
      laser_data_[(start_index + i) % 360] = NAN;
    }

  }
  new_data_  = true;
}

Exemplo n.º 2

Arquivo: LaserScanTask.cpp Projeto: Brazilian-Institute-of-Robotics/simulation-orogen-rock_gazebo

void LaserScanTask::readInput( ConstLaserScanStampedPtr & laserScanMSG )
{ lock_guard<mutex> readGuard(readMutex);
    double range_min = laserScanMSG->scan().range_min();
    double range_max = laserScanMSG->scan().range_max();

    base::samples::LaserScan scan;
    scan.time = getCurrentTime(laserScanMSG->time());
    scan.minRange = meters_to_milimeters(range_min);
    scan.maxRange = meters_to_milimeters(range_max);
    scan.angular_resolution = laserScanMSG->scan().angle_step();
    scan.start_angle = laserScanMSG->scan().angle_min();

    unsigned int scan_size = laserScanMSG->scan().ranges_size();
    scan.ranges.resize(scan_size);
    for(unsigned int i = 0; i < scan_size; ++i) {
        double range = laserScanMSG->scan().ranges(i);

        if (range >= range_max) {
            scan.ranges[i] = base::samples::TOO_FAR;
        }
        else if (range <= range_min) {
            scan.ranges[i] = base::samples::TOO_NEAR;
        }
        else {
            scan.ranges[i] = meters_to_milimeters(range);
        }
    }
    scans.push_back(scan);
}

Exemplo n.º 3

Arquivo: gazebo_ros_gpu_laser.cpp Projeto: ChefOtter/ahl_ros_pkg

////////////////////////////////////////////////////////////////////////////////
// Convert new Gazebo message to ROS message and publish it
void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg)
{
  // We got a new message from the Gazebo sensor.  Stuff a
  // corresponding ROS message and publish it.
  sensor_msgs::LaserScan laser_msg;
  laser_msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec());
  laser_msg.header.frame_id = this->frame_name_;
  laser_msg.angle_min = _msg->scan().angle_min();
  laser_msg.angle_max = _msg->scan().angle_max();
  laser_msg.angle_increment = _msg->scan().angle_step();
  laser_msg.time_increment = 0;  // instantaneous simulator scan
  laser_msg.scan_time = 0;  // not sure whether this is correct
  laser_msg.range_min = _msg->scan().range_min();
  laser_msg.range_max = _msg->scan().range_max();
  laser_msg.ranges.resize(_msg->scan().ranges_size());
  std::copy(_msg->scan().ranges().begin(),
            _msg->scan().ranges().end(),
            laser_msg.ranges.begin());
  laser_msg.intensities.resize(_msg->scan().intensities_size());
  std::copy(_msg->scan().intensities().begin(),
            _msg->scan().intensities().end(),
            laser_msg.intensities.begin());
  this->pub_queue_->push(laser_msg, this->pub_);
}