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

void tacta_output(void) {

//    int lf_lf = lh_lf_output[loc];
//    int lf_rf = lh_rf_output[loc];
//    int rf_lf = rh_lf_output[loc];
//    int rf_rf = rh_rf_output[loc];


//    ROS_WARN("SENDING [%d, %d, %d, %d]", lf_lf, lf_rf, rf_lf, rf_rf);

    data_amp[0] = TACTA_BELT_ADDRESS;
    data_amp[1] = TACTA_BELT_SET_OUTPUT;
    data_amp[2] = TACTA_BELT_ID;
    data_amp[3] = RH_RF;
    data_amp[4] = rh_rf_output[loc];
    data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4];
    device.write(data_amp, 6);

    data_amp[0] = TACTA_BELT_ADDRESS;
    data_amp[1] = TACTA_BELT_SET_OUTPUT;
    data_amp[2] = TACTA_BELT_ID;
    data_amp[3] = RH_LF;
    data_amp[4] = rh_lf_output[loc];
    data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4];
    device.write(data_amp, 6);


    data_amp[0] = TACTA_BELT_ADDRESS;
    data_amp[1] = TACTA_BELT_SET_OUTPUT;
    data_amp[2] = TACTA_BELT_ID;
    data_amp[3] = LH_RF;
    data_amp[4] = lh_rf_output[loc];
    data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4];
    device.write(data_amp, 6);

    data_amp[0] = TACTA_BELT_ADDRESS;
    data_amp[1] = TACTA_BELT_SET_OUTPUT;
    data_amp[2] = TACTA_BELT_ID;
    data_amp[3] = LH_LF;
    data_amp[4] = lh_lf_output[loc];
    data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4];
    device.write(data_amp, 6);

    loc++;

    if (loc >= 100) { loc = 0; }

}

//receive cmds_vel from nav_stack
void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel){
  
	std::string result = drive(cmd_vel->linear.x, cmd_vel->angular.z, ID_Robot);
	ROS_INFO("[cmd_vel]: Vlinear = %f, Vangular = %f",cmd_vel->linear.x, cmd_vel->angular.z);
	ROS_INFO("Sending: %s\n", result.c_str());
	serial_port.write( result.c_str() ); 
}

//Callback for use with the belts msg
void belt_cb (feedback_devices::tacta_belt belt){

  ROS_INFO("GOT A BELT MSG");

  //For Each Motor
  for (int i = 0; i < belt.motors; i++){

    ROS_INFO("MOTOR: %d, SET: %f", i, belt.values[i]);

  }

  ROS_INFO("--------------------");

  //For Each Motor
  for (int i = 0; i < belt.motors; i++){

    //Write the value to the belt  
    data[0] = TACTA_BELT_ADDRESS;
    data[1] = TACTA_BELT_SET_OUTPUT;
    data[2] = TACTA_BELT_ID;
    data[3] = i;
    data[4] = belt.values[i] * 255;
    data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
    device.write(data, 6);
  }

}

/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void cmd_vel_Callback(const geometry_msgs::Twist::ConstPtr& msg)
{
  	double temp_val;

	rec_vel_x = msg->linear.x;
	rec_vel_y = msg->linear.y;
        base_angle = msg->angular.z;
	if(rec_vel_x == 0 && rec_vel_y ==0)
	{
		ang_send[0]='x';
		base_vel = 0.0;
		base_angle = 0;
	}
        else{
		if(base_angle>0){
                    ang_send[0] ='s';
		    base_vel = 0.3;
                }
                else if(base_angle<0){
		    ang_send[0]='a';
                    base_vel=0.3;
                }
                else if(base_angle==0){
                    ang_send[0]='z';
                    base_vel=0.22;
                }
 
        }
  device.write(ang_send,1);  

}

// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{

  ros::init(argc, argv, "tacta_wrist");
  ros::NodeHandle n;
	ros::Publisher state;
  ros::Rate r(TACTA_BELT_STATE_UPDATE);

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_wrist/input", 1, wrist_cb);
  
  // Change the next line according to your port name and baud rate
  try{ device.open("/dev/ttyUSB0", 19200); }
  catch(cereal::Exception& e)
  {
      ROS_FATAL("Failed to open the serial port!!!");
      ROS_BREAK();
  }
  ROS_INFO("The serial port is opened.");
  
  //On Init Enable B-Cast on All Channels
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_ENB_OUTPUT;
  data[2] = TACTA_BELT_BCAST;
  data[3] = TACTA_BELT_BCAST;
  data[4] = data[0] ^ data[1] ^ data[2] ^ data[3];
  device.write(data, 5);

  ros::spin();

}

/**
* @brief   callback fonction pour envoyer la commande a la carte de robot.
* @param   hexapode_v2::sequence_moteur::Request&
* @param   hexapode_v2::sequence_moteur::Response&
* @return  bool
*/
bool sequence_moteur(hexapode_v2::sequence_moteur::Request  &req, hexapode_v2::sequence_moteur::Response &res)
{
    int i = 0;
	for(i = 0; i < 100; i++){
		if (isSerialFree()){
			break;
		}
		else{
			usleep(5000); // 5 millisecondes
		}
	}
    strcpy(sequ, (req.sequence).c_str());

    device.write(sequ, (int)(req.dim));
    ROS_INFO("Ecriture sur le port serie "+req.dim);
	ROS_INFO(sequ);
    // Get the reply, the last value is the timeout in ms
	/*
    try{ device.read(reply, 256, TIMEOUT); }
    catch(cereal::TimeoutException& e)
    {
        ROS_ERROR("Timeout!");
    }
    ROS_INFO("Reponse obtenu du port serie: %s", reply);
	*/
    ros::spinOnce();

    return true;

}

// Topic subscriber callbacks
void commTxCallback(const hexacamina::commData trama)
{
	char dato;
//	ROS_INFO("COMM: recibo dato");
		dato = trama.data_hexapodo;
		cp.write(&dato, 1);

	ROS_INFO("COMM:\n");
	ROS_INFO("[%s]: 0x%X.", COMM_MSG_TX, dato);
}

// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{

  ros::init(argc, argv, "tacta_fingers");
  ros::NodeHandle n;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_grippers/input", 1, grippers_cb);

  // Change the next line according to your port name and baud rate
  try{ device.open("/dev/ttyUSB0", 19200); }
  catch(cereal::Exception& e)
  {
      ROS_FATAL("Failed to open the serial port!!!");
      ROS_BREAK();
  }
  ROS_INFO("The serial port has been opened.");

  //On Init Enable B-Cast on All Channels
  data_amp[0] = TACTA_BELT_ADDRESS;
  data_amp[1] = TACTA_BELT_ENB_OUTPUT;
  data_amp[2] = TACTA_BELT_BCAST;
  data_amp[3] = TACTA_BELT_BCAST;
  data_amp[4] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3];
  device.write(data_amp, 5);

  ros::Rate r(100);

  for (int i = 0; i < 100; i++){
      //Right Hand output struct
      rh_rf_output[i] = 0;
      rh_lf_output[i] = 0;

      //Left Hand output struct
      lh_rf_output[i] = 0;
      lh_lf_output[i] = 0;
  }

  while(ros::ok()){

      tacta_output();

      ros::spinOnce();

      r.sleep();

  }

  ros::spin();

}

// Create a ROS publisher and suscriber for the tacta_belt
int main(int argc, char** argv)
{

  ros::init(argc, argv, "tacta_belt");
  ros::NodeHandle n;
	ros::Publisher state;
  ros::Rate r(TACTA_BELT_STATE_UPDATE);

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = n.subscribe ("/feedback_devices/tacta_belt/input", 1, belt_cb);

  //Advertise the two publishers, one for the commands and one for the gui
	state = n.advertise<feedback_devices::tacta_belt>("/feedback_devices/tacta_belt/state", 1);
  
  // Change the next line according to your port name and baud rate
  try{ device.open("/dev/ttyUSB0", 19200); }
  catch(cereal::Exception& e)
  {
      ROS_FATAL("Failed to open the serial port!!!");
      ROS_BREAK();
  }
  ROS_INFO("The serial port is opened.");
  
  //On Init Enable B-Cast on All Channels
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_ENB_OUTPUT;
  data[2] = TACTA_BELT_BCAST;
  data[3] = TACTA_BELT_BCAST;
  data[4] = data[0] ^ data[1] ^ data[2] ^ data[3];
  device.write(data, 5);

  ros::spin();

//  while(ros::ok())
//  {
//
//    // Get the reply, the last value is the timeout in ms
//    try{ device.read(reply, REPLY_SIZE, TIMEOUT); }
//    catch(cereal::TimeoutException& e)
//    {
//       ROS_ERROR("Timeout!");
//   }
//    ROS_INFO("Got this reply: %s", reply);
//
//    ros::spinOnce();
//    r.sleep();
//  }   
}

/**
* @brief   fonction pour verifier si la porte serie est libre ou pas, ca depend de la carte de robot..
* @param   void
* @return  bool
*/
bool isSerialFree(){
	strcpy(sequ, "Q\r\n");
	device.write(sequ, 3);
	memset(reply, 0, 4);
	try{ 
		device.read(reply, 1, 10); 
	}
    catch(cereal::TimeoutException& e){
        ROS_ERROR("Timeout!");
    }
    ROS_INFO("%s", reply);
	if ('+' == reply[0]){ // busy.
		return false;
	}
	else{//'.', free.
		return true;
	}
}

void tacta_output(void) {

    static int loc = 0;

    for (int channel = 0; channel < CHANNELS; channel++){

        if (active[channel] == feedback_devices::tacta_box::ACTIVE) {

            data_amp[0] = TACTA_BELT_ADDRESS;
            data_amp[1] = TACTA_BELT_SET_OUTPUT;
            data_amp[2] = TACTA_BELT_ID;
            data_amp[3] = channel;
            data_amp[4] = output[channel][loc];
            data_amp[5] = data_amp[0] ^ data_amp[1] ^ data_amp[2] ^ data_amp[3] ^ data_amp[4];
            device.write(data_amp, 6);

        }
    }

    loc++;

    if (loc >= 100) { loc = 0; }

}

int main(int argc, char** argv){ //typical usage: "./traxbot_node /dev/ttyACMx"
  
	ros::init(argc, argv, "traxbot_node");
	ros::NodeHandle n;
	ros::NodeHandle pn("~");
	std::string port;
	
	if (argc<2){
	 port="/dev/ttyACM0";
	 ROS_WARN("No Serial Port defined, defaulting to \"%s\"",port.c_str());
	 ROS_WARN("Usage: \"rosrun [pkg] robot_node /serial_port\"");
	}else{
	  port=(std::string)argv[1];
	  ROS_INFO("Serial port: %s",port.c_str());
	}	
	
	// ROS publishers and subscribers
 	ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 100);
 	tf::TransformBroadcaster odom_broadcaster;
 	ros::Subscriber cmd_vel_sub  = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 10, cmdVelReceived);
    
	odom_pub_ptr = &odom_pub;
	odom_broadcaster_ptr = &odom_broadcaster;	
	
	// baud_rate and serial port:	
	int baudrate;
	pn.param<std::string>("port", port, port.c_str()); 
	pn.param("baudrate", baudrate, 115200); 

	// Open the serial port to the robot
	try{ serial_port.open((char*)port.c_str(), baudrate); }
	catch(cereal::Exception& e){
		ROS_FATAL("Robot -- Failed to open serial port!");
		ROS_BREAK();
	}
    
	//wait (2.5 seconds) until serial port gets ready
	ros::Duration(2.5).sleep(); 	
		
	// Ask Robot ID from the Arduino board (stored in the EEPROM)
	ROS_INFO("Starting Traxbot...");
	serial_port.write("@5e");
	std::string reply;

	try{ serial_port.readBetween(&reply,'@','e'); }	
	catch(cereal::TimeoutException& e){
	  ROS_ERROR("Initial Read Timeout!");
	}
	
	int VDriver, Temperature, OMNI_Firmware, Battery;
	sscanf(reply.c_str(), "@5,%d,%d,%d,%d,%de", &Temperature, &OMNI_Firmware, &Battery, &VDriver, &ID_Robot);   //encoder msg parsing

	ROS_INFO("Traxbot ID = %d", ID_Robot);
	if (ID_Robot < 1 || ID_Robot > 3){
		ROS_WARN("Attention! Unexpected Traxbot ID!");
	}
	ROS_INFO("OMNI Board Temperature = %.2f C", Temperature*0.01);
	ROS_INFO("OMNI Firmware = %.2f", OMNI_Firmware*0.01);
	ROS_INFO("Arduino Firmware Version = %d.00", VDriver);

	if (VDriver > 1500){
		ROS_ERROR("Reset Robot Connection and try again.");
		return(0);
	}

	ROS_INFO("Battery = %.2f V", Battery*0.01 );
	if (Battery < 100){
		ROS_ERROR("Robot is off. Check power source!");
		return(0);
	}
	if (Battery < 850 && Battery > 100){
		ROS_WARN("Warning! Low Battery!");
	}
	
	// Start receiving streaming data
	if( !serial_port.startReadBetweenStream(boost::bind(&robotDataCallback, _1), '@', 'e') ){
		  ROS_FATAL("Robot -- Failed to start streaming data!");
		  ROS_BREAK();
	}
	serial_port.write("@18e");
    
	ros::spin(); //trigger callbacks and prevents exiting
  	return(0);
}

//Callback for use with the belts msg
void wrist_cb (feedback_devices::tacta_wrist wrist){

  ROS_INFO("--------------------");

  //Top
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_SET_OUTPUT;
  data[2] = TACTA_BELT_ID;
  data[3] = 0;
  data[4] = wrist.top;
  data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
  device.write(data, 6);


  //Bottom
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_SET_OUTPUT;
  data[2] = TACTA_BELT_ID;
  data[3] = 1;
  data[4] = wrist.bottom;
  data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
  device.write(data, 6);


  //Left
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_SET_OUTPUT;
  data[2] = TACTA_BELT_ID;
  data[3] = 2;
  data[4] = wrist.left;
  data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
  device.write(data, 6);


  //Right
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_SET_OUTPUT;
  data[2] = TACTA_BELT_ID;
  data[3] = 3;
  data[4] = wrist.right;
  data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
  device.write(data, 6);


  //Front
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_SET_OUTPUT;
  data[2] = TACTA_BELT_ID;
  data[3] = 4;
  data[4] = wrist.front;
  data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
  device.write(data, 6);


  //Back
  data[0] = TACTA_BELT_ADDRESS;
  data[1] = TACTA_BELT_SET_OUTPUT;
  data[2] = TACTA_BELT_ID;
  data[3] = 5;
  data[4] = wrist.back;
  data[5] = data[0] ^ data[1] ^ data[2] ^ data[3] ^ data[4];
  device.write(data, 6);

}