C++ Serial::available方法代码示例

void update_mavlink(void){

    mavlink_message_t msg;
    mavlink_status_t status;

	if(ser.available()){

		uint16_t num_bytes = ser.available();

		uint8_t buffer[num_bytes];

		ser.read(buffer, num_bytes);

		for(uint16_t i = 0; i < num_bytes; i++){

	        if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) {

	            handleMessage(&msg, MAVLINK_COMM_0);

	        }
		}

	}

}

void receive(){
  if(!checkConnection()){
  	return;
	}
  
  // reads:
  //  3 bytes:
  //     1 byte  to compare with START_BYTE
  //     1 bytes into _msg->len
  //     1 byte  into _msg->type
  //  len+1 bytes:
  //   len bytes into _msg->data
  //     1 byte  to compare with _msg->checksum

  
  size_t amount;
  while((amount=port.available()) > 0){
    if(_pos > 0){
      amount = min((unsigned int)amount, _msg->length + 3 - _pos);
      port.read(&(_msg->data[_pos]), amount);
      _pos += amount;

      if(_pos == _msg->length + 3 && port.available()){
        uint8_t checksum;
        port.read(&checksum, 1);
        _msg->calcChecksum();

        if(checksum == _msg->checksum){
          parse(_msg);
        }else{
          ROS_WARN_THROTTLE(1, "Serial: Checksum mismatch: %u != %u", checksum, _msg->checksum);
        }
        resetMessage();
      }
  
    }else if(amount >= 3){
      uint8_t b;
      port.read(&b, 1);

      if(b != START_BYTE){
        ROS_WARN_THROTTLE(1, "Serial: Expected start byte, got this instead: %u", b);
        return;
      }
      uint8_t d[2];
      port.read(d, 2);
      uint16_t len = d[0];
			if(len > 0){
	      _msg = new Message(len - 1, d[1]); // TODO catch exception?
      	_pos = 3;
			}
    }
  }
}

		// Get all bytes available
		bool getBytes(char * buffer, int& buf_len) {
			
			bool res = false;
			//buf_len = 0;
			try {
				while (true) {
						
						int res = 0;
						size_t available = m_serial.available();
						if (!available) {
							break;
						} else {
							res = m_serial.read((uint8_t*)buffer + buf_len, available);
						}
						
						if (res <= 0) {
								break;
						}

						buf_len += res;
						m_last_read_time = Datapoint::get_current_time();


				}

			} catch (...) {
				if (debug) {
					std::cout << "exception thrown while wattsup reading" << std::endl;
				}

				close();

			}
			
			if (buf_len > 0) {
				res = true;
			}

			return res;
		}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "doa_rssi_server");
  ros::NodeHandle n2;
  tf::TransformListener listener;
  ros::Rate r(10);
  ros::ServiceServer service = n2.advertiseService("get_doa_rssi", add);
  ros::Publisher marker_pub_path = n2.advertise<visualization_msgs::Marker>("beacon_point", 10);
  tf::StampedTransform transformMtoR;
  tf::StampedTransform transform;
  string dataString;
  float doarssi[3];
  bool doarssiready = false;
  //ros::spin();//a blocking call duhh!
  ROS_INFO("ready to give DOA and RSSI!!");

   visualization_msgs::Marker markerDOA,markerIntersections,markerBeacon1,markerBeacon2,markerPath;
   markerDOA.header.frame_id  = "/map";
   markerDOA.header.stamp = ros::Time::now();
   markerDOA.ns = "markers";
   markerDOA.action   = visualization_msgs::Marker::ADD;
   markerDOA.pose.orientation.w = 1.0;
   markerDOA.id = 0;
   markerDOA.type = visualization_msgs::Marker::POINTS;
 //########POINTS markers use x and y scale for width/height respectively
   markerDOA.scale.x = 1.2;
   markerDOA.scale.y = 1.2;
   markerDOA.scale.z = 0.5;
   markerDOA.color.r = 1.0f;
   markerDOA.color.b = 1.0f;
   markerDOA.color.a = 1.0;
   geometry_msgs::Point marker_point;
   marker_point.x = 19.0;
   marker_point.y = 19.0;
   marker_point.z = 0.0;
   markerDOA.points.push_back(marker_point);
   marker_pub_path.publish(markerDOA);
  while(n2.ok())
  {
#ifndef SDR
	  try{
		 listener.lookupTransform("base_link", "beacon",
								 ros::Time(0), transform);
	  }
	  catch (tf::TransformException &ex) {
		ROS_ERROR("%s",ex.what());
		ros::Duration(1.0).sleep();
		continue;
	  }
	  phi = atan2f(transform.getOrigin().y(),transform.getOrigin().x());
	  rssi = 1/(transform.getOrigin().x()*transform.getOrigin().x()+transform.getOrigin().y()*transform.getOrigin().y());
#else
	if(robotserial.available())
	{
		dataString = robotserial.readline(100,"\n");
		cout << "odom received: "<< dataString << endl;
		char * dataStringArray = new char [dataString.length()+1];
		std::strcpy (dataStringArray, dataString.c_str());
			//printf("%s\n",dataStringArray);
		// dataStringArray now contains a c-string copy of str
		char * token = std::strtok (dataStringArray,",");
		char *unconverted;
		int i = 0;
		if(*token=='t')
		{
			while (token!=0)
			{
				//std::cout << token << "-" << atoi(token) <<'\n';
				doarssi[i] = atof(token);
				token = std::strtok(NULL,",");
				i++;
			}
			doarssiready = true;
		}
	}
#endif
	if(doarssiready==true)
	{
		phi = doarssi[1];
		rssi = doarssi[2];
		doarssiready = false;
	}
	ros::spinOnce();
	marker_pub_path.publish(markerDOA);
  }
  return 0;
}