C++ serial::Serial类代码示例

bool checkConnection(){
	if(!port.isOpen() && !port.getPort().empty()){
		try{
			port.open();
			ROS_WARN_THROTTLE(5, "Serial port was closed, reopened.");
		} catch (const serial::IOException& ex){
			ROS_ERROR_THROTTLE(10, "Serial port is closed, reopening failed: %s", ex.what());
		}
	}
}

int main(int argc, char **argv){

	ros::init(argc, argv, "nayan_ros");

	ros::NodeHandle n;



	imu_pub = n.advertise<nayan_msgs::nayan_imu>("nayan_ros/imu", 1000);

	debug_var_pub = n.advertise<nayan_msgs::nayan_dbg>("nayan_ros/debug_var", 1000);

	gps_pose_vel_pub = n.advertise<nayan_msgs::nayan_gps_pose_vel>("nayan_ros/gps_pose_vel", 1000);

	attitude_pub = n.advertise<nayan_msgs::nayan_attitude>("nayan_ros/attitude", 1000);

	rc_in_pub = n.advertise<nayan_msgs::nayan_rc_in>("nayan_ros/rc_in", 1000);


	vision_est_sub = n.subscribe("nayan_ros/vision_estimate", 1000, update_vision_estimate);

	pose_vel_setpt_sub = n.subscribe("nayan_ros/setpoint_pose_vel", 1000, update_setpoint_pose_vel);


	param_srv = n.advertiseService("param_set", update_param);


	ros::Duration(2).sleep();

	if (ser.isOpen()){
		ROS_INFO("Serial Port Opened!");
	}else{
		ROS_ERROR("Serial Port Cannot be Opened!");
	}

	ros::Rate loop_rate(100);

	while(ros::ok()){

		update_mavlink();

		ros::spinOnce();

		loop_rate.sleep();
	}


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

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

	        }
		}

	}

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "syncboard_node");
  ros::NodeHandle nh;

  Move mover;

  // Advertise topics
  ros::Publisher chatter_pub = nh.advertise<geometry_msgs::Twist>("/camera_stitch/cmd_vel", 1);
  ros::Publisher timestamps_pub = nh.advertise<std_msgs::String>("syncbox_timestamps", 1);

  //Synboard stuff
  std::string port;
  int baud;
  nh.param<std::string>("port", port, "/dev/ttyUSB0");
  nh.param("baud", baud, 9600);
  syncboard.setTimeout(serial::Timeout::max(), 500, 0, 500, 0);
  syncboard.setPort(port);
  syncboard.setBaudrate(baud);
  syncboard.open();
  mover.handshake();
  while(ros::ok()){
    ros::spinOnce();
    ros::Rate(10).sleep();
  }
  syncboard.write("s");

  return 0;
}

        bool open() {

				bool device_opened = false;

				if (is_open()) {
					if (debug)
						std::cout << "serial already opened" << std::endl;
					
				} else {
				
					m_serial.setPort(m_device_port);
					m_serial.setBaudrate(115200);
					//serial::Timeout t(1000, 1000);
					m_serial.setTimeout(serial::Timeout::simpleTimeout(2000));
					m_serial.open();

					// Open port
					if (m_serial.isOpen()) {
						m_serial.flush();
						// Check if the device is well a Wattsup
						if (identify()) {
							m_serial.flush();
							device_opened = true;
						} else {
							m_serial.close();
						}
					}
				}

				return device_opened;
        }

bool Move::burst_callback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res)
{
  static unsigned int count=0;

  res.success = true; 
  res.message = "Started syncboard burst trigger"; //this trigger defaults to 5hz and is independent of robot position
  geometry_msgs::Twist output;

  output.linear.x = 0.2;		
  output.linear.y = 0.0;
  output.linear.z = 0.0;
  output.angular.x = 0.0;
  output.angular.y = 0.0;
  output.angular.z = 0.0;
  movePub.publish(output);

  syncboard.write("b");

  usleep(1700000); //TUNE THIS PARAMETER

  output.linear.x = 0.0;
  output.linear.y = 0.0;
  output.linear.z = 0.0;
  output.angular.x = 0.0;
  output.angular.y = 0.0;
  output.angular.z = 0.0;
  movePub.publish(output);

  ROS_INFO("Finished the burst");  
  count++;
  return true;
}

static void comm_send_ch(mavlink_channel_t chan, uint8_t ch)
{
    if (chan == MAVLINK_COMM_0)
    {
    	ser.write(&ch, 1);
    }
}

bool Move::handshake(){
  int counter = 0;
  while(ros::ok()){
    syncboard.flush();
    syncboard.write("v");
    std::string result = syncboard.readline();
    if (result.length() > 0){
      ROS_INFO("Connected to syncboard.");
      return true;
    }
    if(counter++ > 50){
      ROS_WARN_ONCE("Connecting to syncboard is taking longer than expected.");
    }
    ros::Rate(10).sleep();
  }
  ROS_WARN("Syncboard handshake failed.");
  return false;  
}

void sendCommand(int robotId, int leftMotorSpeed, int rightMotorSpeed)
{
        char cmd[4];
        cmd[0] = robotId;
        cmd[1] = 128 + leftMotorSpeed;
        cmd[2] = 128 + rightMotorSpeed;
        cmd[3] = '\n';
        string cmdStr(cmd, cmd + 4);
        my_serial.write(cmdStr);
}

// Opens and configures the com-port
void configure_com_port(void)
{
  if (my_serial.isOpen())
  {
    cout << "Port is open " << endl;
  }
  else
  {
     cout << "Unable to open com-port " << endl;
  }
}

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

        bool write(Packet& p) {

				if (!is_open()) {
					close();
					return false;
				}

                memset(p.m_buf, 0, sizeof(p.m_buf));
                int n = sprintf(p.m_buf, "#%c,%c,%d", p.m_cmd, p.m_sub_cmd, p.m_count);
                char* s = p.m_buf + n;
                p.m_len = n;

                for (int i = 0; i < p.m_count; i++) {
                        if ((p.m_len + strlen(p.m_fields[i]) + 4) >= sizeof(p.m_buf)) {
                                return false;
                        }

                        n = sprintf(s, ",%s", p.m_fields[i]);
                        s += n;
                        p.m_len += n;
                }

                p.m_buf[p.m_len++] = ';';

                if (debug) {
                        cout << "[debug-write]: " << m_device_port << ": "
                             << p.m_buf << endl;
                }


				try {
					int pos = 0;
					while (pos < p.m_len) {
							int res = m_serial.write((const uint8_t *)p.m_buf + pos, p.m_len - pos);
							if (res < 0) {
									return false;
							}

							pos += res;
					}
				} catch (...) {
					if (debug) {
						std::cout << "exception thrown while wattsup writing" << std::endl;
					}
				

					close();
				}

                return true;
        }

        void close() {

                if (m_closing) return;
                m_closing = true;

				if (is_open()) {
                        // Internal logging 600s
                        Packet p;
						WattsupCommand::SetupInternalLogging600s(p);

						write(p);

                        m_serial.flush();
						
				}

				m_serial.close();

                m_buf_len = 0;
                m_commands.clear();
                m_closing = false;

				//this->fileLog("Device closed ! [" + m_device_port + "]");
        }

// Sends the given channel values with it's checksum to
// the rc-controller
void send_to_controller( const QuadroController::channel_values::ConstPtr& msg)
{
  send_data[0] = 0x00;
  send_data[1] = 0x01;
  send_data[2] = msg->channel_1;
  send_data[3] = msg->channel_2;
  send_data[4] = msg->channel_3;
  send_data[5] = msg->channel_4;
  send_data[6] = msg->channel_5;
  send_data[7] = 0x00;
  send_data[8] = 0x00;
  send_data[9] = calculate_checksum(send_data,9);
  // 0x55 is the start-byte to initiate the communication
  // prozess 
  send_data[0] = 0x55;
  my_serial.write(send_data,sizeof(send_data));
  // Uncomment to see the checksum
  //ROS_INFO("Checksum: [%d]", send_data[9]);
}