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

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

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

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


     imu_pub = n.advertise<sensor_msgs::Imu>("/imu/data",100);


    // Change the next line according to your port name and baud rate
    try
    {

        if(device.isOpen())
           {
               ROS_INFO("Port is opened");
          }
    }
    catch(serial::SerialException& e)
    {
        ROS_FATAL("Failed to open the serial port!!!");
        ROS_BREAK();
    }

       ros::Rate r(50);
          while(ros::ok())
          {


       // Get the reply, the last value is the timeout in ms
       try{
     std::string reply;

        // ros::Duration(0.005).sleep();

           device.readline(reply);
           Roll = strtod(reply.c_str(),&pRoll)/2;
           Pitch = strtod(pRoll,&pRoll)/2;
           Yaw  = strtod(pRoll,&pRoll)/2;
           GyroX = strtod(pRoll,&pRoll);
           GyroY = strtod(pRoll,&pRoll);
           GyroZ = strtod(pRoll,&pRoll);
           AccX = strtod(pRoll,&pRoll);
           AccY = strtod(pRoll,&pRoll);
           AccZ = strtod(pRoll,&pRoll);
           Mx = strtod(pRoll,&pRoll);
           My = strtod(pRoll,&pRoll);
           Mz = strtod(pRoll,NULL);

           ROS_INFO("Euler %.2f %.2f %.2f",Roll,Pitch,Yaw);

	   sensor_msgs::Imu imu;

           geometry_msgs::Quaternion Q;
           Q = tf::createQuaternionMsgFromRollPitchYaw(Roll*-1,Pitch*-1,Yaw*-1);

           imu.orientation.x = 0;
           imu.orientation.y = 0;
           imu.orientation.z = 0;
           imu.orientation.w = 1;


           LinearX =  (AccX/256)*9.806;
           LinearY = (AccY/256)*9.806;
           LinearZ = (AccZ/256)*9.806;

           imu.angular_velocity.x = GyroX*-1*0.07*0.01745329252;;
           imu.angular_velocity.y = GyroY*0.07*0.01745329252;;
           imu.angular_velocity.z = GyroZ*-1*0.07*0.01745329252;;

           imu.linear_acceleration.x =  LinearX*-1;
           imu.linear_acceleration.y =  LinearY;
           imu.linear_acceleration.z =  LinearZ*-1;
           imu.header.stamp = ros::Time::now();
           imu.header.frame_id = "imu";
           imu_pub.publish(imu);


       }
       catch(serial::SerialException& e)
       {
           ROS_INFO("Error %s",e.what());
       }

              r.sleep();
    }

}