C++ ArRobot::getSonarReading方法代码示例

void readSonars(ArRobot& robot, int numSonar){
	char angle[64], value[64];
	G_id += 1;
	ArSensorReading* sonarReading;
	string res;
	sprintf(value,"id=%d;",G_id);
	res += value;
	for (int i=0; i < numSonar; i++){
		sonarReading = robot.getSonarReading(i);
		sprintf(value,"v%d=%05d;", i, sonarReading->getRange());
		res += value;
		//cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
	}
	res += "\n";
	fseek(G_SONAR_FD, SEEK_SET, 0);
	fwrite(res.c_str(), sizeof(char), res.size(), G_SONAR_FD);
}

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

  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);
  }

}

//.........这里部分代码省略.........
      pose.setPose(myRobot->getX() + 1000, myRobot->getY(), 0);
      myRobot->moveTo(pose);
      break;
    case 3:
      printf("Doing a transform test....\n");
      printf("\nOrigin should be transformed to the robots coords.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(0, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
    
      printf("\nRobot coords should be transformed to the origin.\n");
      transform = myRobot->getToLocalTransform();
      pose = myRobot->getPose();
      pose = transform.doTransform(pose);
      rpose.setPose(0, 0, 0);
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();
      if (pose.findDistanceTo(rpose) < .1)
	printf("Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 4:
      printf("Doing a tranform test...\n");
      printf("A point 1 meter to the -x from the robot (in local coords) should be transformed into global coordinates.\n");
      transform = myRobot->getToGlobalTransform();
      pose.setPose(-1000, 0, 0);
      pose = transform.doTransform(pose);
      rpose = myRobot->getPose();
      printf("Pos:  ");
      pose.log();
      printf("Robot:  ");
      rpose.log();

      if (ArMath::fabs(pose.findDistanceTo(rpose) - 1000.0) < .1)
	printf("Probable Success\n");
      else
	printf("#### FAILURE\n");
      break;
    case 5:
      printf("Doing a transform test on range devices..\n");
      printf("Moving the robot +4 meters x and +4 meters y and seeing if the moveTo will move the sonar readings along with it.\n");
      dev = myRobot->findRangeDevice("sonar");
      if (dev == NULL)
      {
	printf("No sonar on the robot, can't do the test.\n");
	break;
      }
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }
      pose = myRobot->getPose();
      pose.setX(pose.getX() + 4000);
      pose.setY(pose.getY() + 4000);
      myRobot->moveTo(pose);
      printf("Moved robot.\n");
      printf("Closest sonar reading to the robot is %.0f away\n", dev->currentReadingPolar(1, 0));
      printf("Sonar 0 reading is at ");
      son = myRobot->getSonarReading(0);
      if (son != NULL)
      {
	pose = son->getPose();
	pose.log();
      }

      break;
    case 6:
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      printf("Disconnecting from the robot, then reconnecting.\n");
      myRobot->disconnect();
      myRobot->blockingConnect();      
      printf("Robot position now is:\n");
      pose = myRobot->getPose();
      pose.log();
      break;
    default:
      printf("No test for second button.\n");
      break;
    } 
  }
}

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


  // publishing transform odom->base_link
  odom_trans.header.stamp = ros::Time::now();
  odom_trans.header.frame_id = frame_id_odom;
  odom_trans.child_frame_id = frame_id_base_link;
  
  odom_trans.transform.translation.x = pos.getX()/1000;
  odom_trans.transform.translation.y = pos.getY()/1000;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
  
  odom_broadcaster.sendTransform(odom_trans);
  
  // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
  int stall = robot->getStallValue();
  unsigned char front_bumpers = (unsigned char)(stall >> 8);
  unsigned char rear_bumpers = (unsigned char)(stall);

  bumpers.header.frame_id = frame_id_bumper;
  bumpers.header.stamp = ros::Time::now();

  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;
  }

  if (robot->areSonarsEnabled())
  {
    int i = 0;
    int j = 0;
    ArSensorReading* reading = NULL;
    if(sonars__crossed_the_streams)
    {
      i = 8;
      j = 8;
    }
    
    for(; i < 16; i++)
    {
      ranges.data[i].header.stamp = ros::Time::now();
      
      ArSensorReading* _reading = NULL;
      _reading = robot->getSonarReading(i-j);
      ranges.data[i].range = _reading->getRange() / 1000.0f;
      range_pub[i].publish(ranges.data[i]);
    }
    ranges.header.stamp = ros::Time::now();
    combined_range_pub.publish(ranges);
  }  
}

//.........这里部分代码省略.........
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f", robot->getTransVelMax(), robot->getRotVelMax(), robot->getTransAccel(), robot->getTransDecel(), robot->getRotAccel(), robot->getRotDecel());

  ROS_ERROR("IN DEFAULT CONFIG\n\
Trans vel max: %f\n\
Rot vel max: %f\n\
\n\
trans accel: %f\n\
trans decel: %f\n\
rot accel: %f\n\
rot decel: %f\n", dynConf_default.trans_vel_max,  dynConf_default.rot_vel_max, dynConf_default.trans_accel, dynConf_default.trans_decel, dynConf_default.rot_accel, dynConf_default.rot_decel);*/

  TicksMM = robot->getOrigRobotConfig()->getTicksMM();
  DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
  RevCount = robot->getOrigRobotConfig()->getRevCount();

  dynConf_default.TicksMM     = TicksMM;
  dynConf_default.DriftFactor = DriftFactor;
  dynConf_default.RevCount    = RevCount;
  
  dynamic_reconfigure_server->setConfigDefault(dynConf_default);

  for(int i = 0; i < 16; i++)
  {
    sonar_tf_array[i].header.frame_id = frame_id_base_link;
    std::stringstream _frame_id;
    _frame_id << "sonar" << i;
    sonar_tf_array[i].child_frame_id = _frame_id.str();
    ArSensorReading* _reading = NULL;
    _reading = robot->getSonarReading(i);
    sonar_tf_array[i].transform.translation.x = _reading->getSensorX() / 1000.0;
    sonar_tf_array[i].transform.translation.y = _reading->getSensorY() / 1000.0;
    sonar_tf_array[i].transform.translation.z = 0.19;
    sonar_tf_array[i].transform.rotation = tf::createQuaternionMsgFromYaw(_reading->getSensorTh() * M_PI / 180.0);
  }

  for (int i=0;i<16;i++) {
      sensor_msgs::Range r;
      ranges.data.push_back(r);
  }

  int i=0,j=0;
  if (sonars__crossed_the_streams) {
    i=8;
    j=8;
  }
  for(; i<16; i++) {
    //populate the RangeArray msg
    std::stringstream _frame_id;
    _frame_id << "sonar" << i;
    ranges.data[i].header.frame_id = _frame_id.str();
    ranges.data[i].radiation_type = 0;
    ranges.data[i].field_of_view = 0.2618f; 
    ranges.data[i].min_range = 0.03f;
    ranges.data[i].max_range = 5.0f;
  }

  // Enable the motors
  robot->enableMotors();

  robot->disableSonar();