C++ ArSensorReading::getLocalX方法代码示例

/**
 * @brief publish sonar readings
 */
void AriaCore::publish_sonar()
{
    sensor_msgs::PointCloud msg;

    msg.header.stamp    = ros::Time::now();
    msg.header.frame_id = m_frameIDSonar;

    m_robot.lock();
    {
        int i;
        ArSensorReading* pRead;
        for (i=0; i<m_robot.getNumSonar(); ++i) {
            pRead = m_robot.getSonarReading(i);
            if (!pRead) {
                ROS_WARN("Did not receive a sonar reading on %d", i);
                continue;
            }

            geometry_msgs::Point32 point;
            point.x = pRead->getLocalX();
            point.y = pRead->getLocalY();
            point.z = 0.0;

            msg.points.push_back(point);
        }
    }
    m_robot.unlock();

    m_pubSonar.publish(msg);
}

//.........这里部分代码省略.........
        fprintf(myFile, "scan1Id: %d\n", myScanNumber);
        fprintf(myFile, "time: %ld.%ld\n", msec / 1000, msec % 1000);
        /* ScanStudio isn't using these yet so don't print them
          fprintf(myFile, "velocities: %.2f %.2f\n", myRobot->getRotVel(),
          myRobot->getVel());*/
        internalPrintPos(poseTaken);

        if (myUseReflectorValues)
        {
            fprintf(myFile, "reflector1: ");

            if (!mySick->isLaserFlipped())
            {
                // make sure that the list is in increasing order
                for (it = readings->begin(); it != readings->end(); it++)
                {
                    reading = (*it);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%d ", reading->getExtraInt());
                    else
                        fprintf(myFile, "0 ");
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%d ", reading->getExtraInt());
                    else
                        fprintf(myFile, "0 ");
                }
            }
            fprintf(myFile, "\n");
        }
        /**
           Note that the the sick1: or scan1: must be the last thing in
           that timestamp, ie that you should put any other data before
           it.
         **/
        if (myOldReadings)
        {
            fprintf(myFile, "sick1: ");

            if (!mySick->isLaserFlipped())
            {
                // make sure that the list is in increasing order
                for (it = readings->begin(); it != readings->end(); it++)
                {
                    reading = (*it);
                    fprintf(myFile, "%d ", reading->getRange());
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    fprintf(myFile, "%d ", reading->getRange());
                }
            }
            fprintf(myFile, "\n");
        }
        if (myNewReadings)
        {
            fprintf(myFile, "scan1: ");

            if (!mySick->isLaserFlipped())
            {
                // make sure that the list is in increasing order
                for (it = readings->begin(); it != readings->end(); it++)
                {
                    reading = (*it);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%.0f %.0f  ",
                                reading->getLocalX() - mySick->getSensorPositionX(),
                                reading->getLocalY() - mySick->getSensorPositionY());
                    else
                        fprintf(myFile, "0 0  ");
                }
            }
            else
            {
                for (rit = readings->rbegin(); rit != readings->rend(); rit++)
                {
                    reading = (*rit);
                    if (!reading->getIgnoreThisReading())
                        fprintf(myFile, "%.0f %.0f  ",
                                reading->getLocalX() - mySick->getSensorPositionX(),
                                reading->getLocalY() - mySick->getSensorPositionY());
                    else
                        fprintf(myFile, "0 0  ");
                }
            }
            fprintf(myFile, "\n");
        }
        mySick->unlockDevice();
    }
}

//.........这里部分代码省略.........

  std::stringstream bumper_info(std::stringstream::out);
  // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
  for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
  {
    bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
    bumper_info << " " << (front_bumpers & (1 << (i+1)));
  }
  ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());

  bumper_info.str("");
  // Rear bumpers have reverse order (rightmost is LSB)
  unsigned int numRearBumpers = robot->getNumRearBumpers();
  for (unsigned int i=0; i<numRearBumpers; i++)
  {
    bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
    bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
  }
  ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
  
  bumpers_pub.publish(bumpers);

  //Publish battery information
  // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
  std_msgs::Float64 batteryVoltage;
  batteryVoltage.data = robot->getRealBatteryVoltageNow();
  voltage_pub.publish(batteryVoltage);

  if(robot->haveStateOfCharge())
  {
    std_msgs::Float32 soc;
    soc.data = robot->getStateOfCharge()/100.0;
    state_of_charge_pub.publish(soc);
  }

  // publish recharge state if changed
  char s = robot->getChargeState();
  if(s != recharge_state.data)
  {
    ROS_INFO("RosAria: publishing new recharge state %d.", s);
    recharge_state.data = s;
    recharge_state_pub.publish(recharge_state);
  }

  // publish motors state if changed
  bool e = robot->areMotorsEnabled();
  if(e != motors_state.data || !published_motors_state)
  {
	ROS_INFO("RosAria: publishing new motors state %d.", e);
	motors_state.data = e;
	motors_state_pub.publish(motors_state);
	published_motors_state = true;
  }

  // Publish sonar information, if enabled.
  if (use_sonar) {
    sensor_msgs::PointCloud cloud;	//sonar readings.
    cloud.header.stamp = position.header.stamp;	//copy time.
    // sonar sensors relative to base_link
    cloud.header.frame_id = frame_id_sonar;
    

    // Log debugging info
    std::stringstream sonar_debug_info;
    sonar_debug_info << "Sonar readings: ";
    for (int i = 0; i < robot->getNumSonar(); i++) {
      ArSensorReading* reading = NULL;
      reading = robot->getSonarReading(i);
      if(!reading) {
        ROS_WARN("RosAria: Did not receive a sonar reading.");
        continue;
      }
      
      // getRange() will return an integer between 0 and 5000 (5m)
      sonar_debug_info << reading->getRange() << " ";

      // local (x,y). Appears to be from the centre of the robot, since values may
      // exceed 5000. This is good, since it means we only need 1 transform.
      // x & y seem to be swapped though, i.e. if the robot is driving north
      // x is north/south and y is east/west.
      //
      //ArPose sensor = reading->getSensorPosition();  //position of sensor.
      // sonar_debug_info << "(" << reading->getLocalX() 
      //                  << ", " << reading->getLocalY()
      //                  << ") from (" << sensor.getX() << ", " 
      //                  << sensor.getY() << ") ;; " ;
      
      //add sonar readings (robot-local coordinate frame) to cloud
      geometry_msgs::Point32 p;
      p.x = reading->getLocalX() / 1000.0;
      p.y = reading->getLocalY() / 1000.0;
      p.z = 0.0;
      cloud.points.push_back(p);
    }
    ROS_DEBUG_STREAM(sonar_debug_info.str());
    
    sonar_pub.publish(cloud);
  }

}