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

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

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

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

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

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

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

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

//.........这里部分代码省略.........
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_LOADING);
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ERROR);
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_OUTOFBRICKS);
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_SORTING);
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_ORDERSORTED);
			set_rsd_info(INFO_TYPE_STATUS, INFO_ACTION_CLEAR, STATUS_ORDER_COMPLETE);

			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_FREE);
			break;
		case SORTING:
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_CLEAR, MES_STATE_FREE);
			set_rsd_info(INFO_TYPE_MES_STATE, INFO_ACTION_SET, MES_STATE_SORTING);

			if(!orderQueue.empty() && !workingOnOrder)
			{
				currentOrder = orderQueue.front();
				nrOfBricksInOrder = currentOrder.get_blueBricks() + currentOrder.get_redBricks() + currentOrder.get_yellowBricks();
				workingOnOrder = true;
				startBatchTime = ros::Time::now().toSec();
				LINFO << "Working on Order: " << currentOrder.get_orderID();
			}

			switch (control_state)
			{
				case INIT:
				{
					if (linearPath.empty())
					{
						rw::common::Log::log().info() << "In State: " << "INIT" << std::endl;
						Q gripperInitOpenQ(1, 0.020);
						sendQtoGripper(gripperInitOpenQ);

						//Start  conveyor belt
						PLCserial.write("f2");

						// Issue move command for moving robot to ready position and switch state to MOVING_TO_READY_POS
						Frame::Ptr objectFrame = _rwWorkCell->findFrame("Lego1x6");
						MovableFrame::Ptr objectMovFrame = objectFrame.cast<MovableFrame>();

						Vector3D<> newPos(-0.60, 0.135, 0.007);
						RPY<> newRot(0, 0, -Pi);
						Transform3D<> newTarget = Transform3D<>(newPos, newRot);
						objectMovFrame->setTransform(newTarget, _state);

						Transform3D<> baseToObject = _device->baseTframe(_rwWorkCell->findFrame("Lego1x6"), _state);

						// Clear any previous planned path before generating a new
						linearPath.clear();
						moveToTarget(baseToObject, -0.15, false);
					}

					//if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_MOVEROBOT) {
						target_type = READY;
						moveRobot();
						last_state = INIT;
						rw::common::Log::log().info() << "In State: " << "MOVING_TO_READY_POS" << std::endl;
						control_state = MOVING_TO_READY_POS;
					//}
				}
					break;

				case MOVING_TO_READY_POS:
					// Is ready position is reached?
					if (get_rsd_info(INFO_TYPE_STATUS) & STATUS_READY_REACHED) {
						Q gripperInitOpenQ(1, 0.020);
						sendQtoGripper(gripperInitOpenQ);

bool Move::trigger_callback(std_srvs::Trigger::Request  &req, std_srvs::Trigger::Response &res)
{
  syncboard.write("t");
  return true;
}

void transmit(const uint8_t* d, size_t l){
  if(!checkConnection()){
  	return;
	}
	port.write(d, l);
}