void processMocapData( const char** mocap_model )
{
  //UdpMulticastSocket multicast_client_socket( LOCAL_PORT, MULTICAST_IP );
		
  MOCAPSocket Socket;

  static tf::TransformBroadcaster br;

  //ushort payload;
  //MoCapDataFormat format;
  while( true )
  {	
    //int numberOfPackets = 0;
    //bool packetread = false;

    //int trials = 0;

    //while( !packetread && trials < 100 )
    //{
      //int numBytes = 0;

      //do
      //{
	// Receive data form mocap device
	//numBytes = multicast_client_socket.recv();

	// Parse mocap data
	//if( numBytes > 0 )
	//{
	  //const char* buffer = multicast_client_socket.getBuffer();
	  //unsigned short header = *((unsigned short*)(&buffer[0]));
					
	  // Look for the beginning of a NatNet package
	  //if (header == 7)
	  //{
	    //payload = *((ushort*) &buffer[2]);
	    //format.parse(buffer,payload);
	    //packetread = true;
	    //numberOfPackets++;
	  //}
	  // else skip packet
	//}


      //} while( numBytes > 0 );

      // Don't try again immediately
      //if( !packetread )
      //{
	//usleep( 10 );
	//trials++;
      //}
    //}
		
    // Once a mocap package has been received and parsed, publish the data using tf
    if(Socket.Read()>0)
    //if( packetread )
    {
      ros::Time timestamp( ros::Time::now() );
			
      //for( int i = 0; i < format.model[0].numRigidBodies; i++ )
      //{

	tf::Transform transform;
	// Translate mocap data from mm --> m to be compatible with rviz

	// Change y and z due to mocap calibration

	transform.setOrigin( tf::Vector3(-Socket.rigidBody[0].x / 1000.0f,Socket.rigidBody[0].z / 1000.0f,Socket.rigidBody[0].y / 1000.0f ) );

	// Change y and z due to mocap calibration
			    
	tf::Quaternion q( Socket.rigidBody[0].qx, Socket.rigidBody[0].qz, Socket.rigidBody[0].qy, Socket.rigidBody[0].qw ) ;
			    
	transform.setRotation(q.inverse());		// Handle 

	int rigid_body_id = abs(Socket.rigidBody[0].ID);
	const char* rigid_body_name = "OBJECT"; //mocap_model[rigid_body_id];
	br.sendTransform(tf::StampedTransform(transform, timestamp, "base_link", std::string( rigid_body_name ) ));

      //}
    }
  }
}

// Publish the current tracker pose
void SystemFrontendBase::PublishPose()
{
  ROS_ASSERT(mpTracker);

  if (mpTracker->GetTrackingQuality() == Tracker::NONE || mpTracker->IsLost())
    return;

  geometry_msgs::PoseWithCovarianceStamped pose_cov_msg;
  pose_cov_msg.header.stamp = mpTracker->GetCurrentTimestamp();
  pose_cov_msg.header.frame_id = "vision_world";

  TooN::SE3<> pose = mpTracker->GetCurrentPose().inverse();
  TooN::Matrix<3> rot = pose.get_rotation().get_matrix();
  TooN::Matrix<6> cov = mpTracker->GetCurrentCovariance();

  // clear cross correlation
  cov.slice<0, 3, 3, 3>() = TooN::Zeros;
  cov.slice<3, 0, 3, 3>() = TooN::Zeros;

  // Change covariance matrix frame from camera to world
  cov.slice<0, 0, 3, 3>() = rot * cov.slice<0, 0, 3, 3>() * rot.T();
  cov.slice<3, 3, 3, 3>() = rot * cov.slice<3, 3, 3, 3>() * rot.T();

  // Some hacky heuristics here
  if (mpTracker->GetTrackingQuality() == Tracker::GOOD)
  {
    cov = cov * 1e2;
  }
  else if (mpTracker->GetTrackingQuality() == Tracker::DODGY)
  {
    cov = cov * 1e5;
  }
  else if (mpTracker->GetTrackingQuality() == Tracker::BAD)
  {
    cov = cov * 1e8;
  }

  pose_cov_msg.pose.pose = util::SE3ToPoseMsg(pose);

  int numElems = 6;
  for (int i = 0; i < numElems; ++i)
  {
    for (int j = 0; j < numElems; ++j)
    {
      pose_cov_msg.pose.covariance[i * numElems + j] = cov(i, j);
    }
  }

  mPoseWithCovPub.publish(pose_cov_msg);

  static tf::TransformBroadcaster br;
  tf::Transform transform;
  tf::poseMsgToTF(pose_cov_msg.pose.pose, transform);
  br.sendTransform(tf::StampedTransform(transform, mpTracker->GetCurrentTimestamp(), "vision_world", "vision_pose"));

  SE3Map mPoses = mpTracker->GetCurrentCameraPoses();

  geometry_msgs::PoseArray pose_array_msg;
  pose_array_msg.header.stamp = pose_cov_msg.header.stamp;
  pose_array_msg.header.frame_id = pose_cov_msg.header.frame_id;
  pose_array_msg.poses.resize(1 + mPoses.size());
  pose_array_msg.poses[0] = pose_cov_msg.pose.pose;

  int i = 1;
  for (SE3Map::iterator it = mPoses.begin(); it != mPoses.end(); ++it, ++i)
  {
    pose_array_msg.poses[i] = util::SE3ToPoseMsg(it->second.inverse());
    tf::poseMsgToTF(pose_array_msg.poses[i], transform);
    br.sendTransform(tf::StampedTransform(transform, mpTracker->GetCurrentTimestamp(), "vision_world", it->first));
  }

  mPoseArrayPub.publish(pose_array_msg);
}

void* ControlThread(void *p)
{
	COdometerCapture *pComm = (COdometerCapture*)p;

	/// Handling events
	const int EVENT_NUM = 4;
	HANDLE hWait[EVENT_NUM];
	hWait[0] = pComm->m_hEventOdoCalTimer;
	hWait[1] = pComm->m_hEventOdoPubTimer;
	hWait[2] = pComm->m_hEventSpeedTimer;
	hWait[3] = pComm->m_hEventSpeedCheckTimer;

	/// Real Send Speed
//	double real_vx = 0.0;
//	double real_vy = 0.0;
//	double real_w = 0.0;
	// allowance change value for a single cycle
//	double allow_vx_change_per_cycle = 2.5;		// cm/s
//	double allow_vx_decrease_per_cycle = 5.75;    //cm/s
//	double allow_vy_change_per_cycle = 2.5;		// cm/s
//	double allow_w_increase_per_cycle = 4.5;		// deg/s
//	double allow_w_decrease_per_cycle = 4.5;		// deg/s
//
//	extern double g_closest_obstacle_x;  //urg看到的最近障碍物点 x,y坐标
//	extern double g_closest_obstacle_y;  //urg看到的最近障碍物点
//	extern double g_max_acc_inc_x;  //最大加速度
//	extern double g_max_acc_dec_x;  //最减加速度
//	extern double g_max_acc_w;
//	extern double g_max_vel_x; //最大速度
//	extern double g_max_vel_w; //最大角速度
	extern boost::mutex g_mutex_obstacle; //

//	double cur_ac_vx,cur_ac_vy,cur_ac_vw;
//	double old_x = 0,old_y = 0,old_w = 0;
	boost::posix_time::ptime prev_odom_time;
	boost::mutex vel_mutex;


	/// Handling loop
	fd_set fdset;
	//uint64_t exp;
	struct timeval timeout;
	timeout.tv_sec=5;
	timeout.tv_usec=0;
	while (pComm->m_initial)
	{
        FD_ZERO(&fdset);
        int maxfp=0;
	    for(int k=0;k<EVENT_NUM;k++)
	    {
	        FD_SET(hWait[k],&fdset);
	        if(maxfp<hWait[k])
               {
                    maxfp=hWait[k];
               }
	    }
	    int dRes = select(maxfp+1,&fdset,NULL,NULL,&timeout);
		switch (dRes)
		{
    //*********************************//
    //        修改case后面的值                     //
    //********************************//
        case -1 :
                cout<<"error of select"<<endl;
        case 0 :
                cout<<"No Data within five seconds"<<endl;
		default :
		    if(FD_ISSET(hWait[0],&fdset))						// 里程计合成
			{
				pComm->m_mutex_cap.Lock();
				ODOCAL->onTimerCal2(); /// Timer on calculation - using position
				pComm->m_mutex_cap.Unlock();
			}
            if(FD_ISSET(hWait[1],&fdset))						// 里程计发布
			{
				pComm->m_mutex_cap.Lock();
                nav_msgs::Odometry odometerData;
                pComm->GetCurOdometry(odometerData);
                odometerData.header.stamp = ros::Time::now();
                odometerData.header.frame_id = "odom";
                odometerData.child_frame_id = "base_link";
                static tf::TransformBroadcaster odom_broadcaster;
                geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometerData.pose.pose.orientation.z);
                ///first, we'll publish odom the transform over tf
                geometry_msgs::TransformStamped odom_trans;
                odom_trans.header.stamp = ros::Time::now();
                odom_trans.header.frame_id = "odom";
                odom_trans.child_frame_id = "base_link";

                odom_trans.transform.translation.x = odometerData.pose.pose.position.x;
                odom_trans.transform.translation.y = odometerData.pose.pose.position.y;
                odom_trans.transform.translation.z = 0.0;
                odom_trans.transform.rotation = odom_quat;
                ///send the transform
                odom_broadcaster.sendTransform(odom_trans);
                ///publish laser transform over tf
                ////sensor_msgs::LaserScan LaserData;
               // pComm->GetCurLaser(LaserData);
                static tf::TransformBroadcaster laser_broadcaster;
                tf::Transform laser_transform;
//.........这里部分代码省略.........

    bool detectFiducials(cob_object_detection_msgs::DetectionArray& detection_array, cv::Mat& color_image)
    {
        int id_start_idx = 2351;
        unsigned int marker_array_size = 0;
        unsigned int pose_array_size = 0;

        // Detect fiducials
        std::vector<ipa_Fiducials::t_pose> tags_vec;
        std::vector<std::vector<double> >vec_vec7d;
        if (m_pi_tag->GetPose(color_image, tags_vec) & ipa_Utils::RET_OK)
        {
            pose_array_size = tags_vec.size();

            // TODO: Average results
            for (unsigned int i=0; i<pose_array_size; i++)
            {
                cob_object_detection_msgs::Detection fiducial_instance;
                fiducial_instance.label = "pi-tag"; //tags_vec[i].id;
                fiducial_instance.detector = "Fiducial_PI";
                fiducial_instance.score = 0;
                fiducial_instance.bounding_box_lwh.x = 0;
                fiducial_instance.bounding_box_lwh.y = 0;
                fiducial_instance.bounding_box_lwh.z = 0;

                // TODO: Set Mask
                cv::Mat frame(3,4, CV_64FC1);
                for (int k=0; k<3; k++)
                    for (int l=0; l<3; l++)
                        frame.at<double>(k,l) = tags_vec[i].rot.at<double>(k,l);
                frame.at<double>(0,3) = tags_vec[i].trans.at<double>(0,0);
                frame.at<double>(1,3) = tags_vec[i].trans.at<double>(1,0);
                frame.at<double>(2,3) = tags_vec[i].trans.at<double>(2,0);
                std::vector<double> vec7d = FrameToVec7(frame);
                vec_vec7d.push_back(vec7d);

                // Results are given in CfromO
                fiducial_instance.pose.pose.position.x =  vec7d[0];
                fiducial_instance.pose.pose.position.y =  vec7d[1];
                fiducial_instance.pose.pose.position.z =  vec7d[2];
                fiducial_instance.pose.pose.orientation.w =  vec7d[3];
                fiducial_instance.pose.pose.orientation.x =  vec7d[4];
                fiducial_instance.pose.pose.orientation.y =  vec7d[5];
                fiducial_instance.pose.pose.orientation.z =  vec7d[6];

                fiducial_instance.pose.header.stamp = received_timestamp_;
                fiducial_instance.pose.header.frame_id = received_frame_id_;

                detection_array.detections.push_back(fiducial_instance);
                ROS_INFO("[fiducials] Detected PI-Tag '%s' at x,y,z,rw,rx,ry,rz ( %f, %f, %f, %f, %f, %f, %f ) ",
                         fiducial_instance.label.c_str(), vec7d[0], vec7d[1], vec7d[2],
                         vec7d[3], vec7d[4], vec7d[5], vec7d[6]);
            }
        }
        else
        {
            pose_array_size = 0;
        }

        // Publish 2d image
        if (publish_2d_image_)
        {
            for (unsigned int i=0; i<pose_array_size; i++)
            {
                RenderPose(color_image, tags_vec[i].rot, tags_vec[i].trans);

                cv_bridge::CvImage cv_ptr;
                cv_ptr.image = color_mat_8U3_;
                cv_ptr.encoding = CobFiducialsNode::color_image_encoding_;
                img2D_pub_.publish(cv_ptr.toImageMsg());
            }
        }

        // Publish tf
        if (publish_tf_)
        {
            for (unsigned int i=0; i<pose_array_size; i++)
            {
                // Broadcast transform of fiducial
                tf::Transform transform;
                std::stringstream tf_name;
                tf_name << "pi_tag" <<"_" << "0";
                transform.setOrigin(tf::Vector3(vec_vec7d[i][0], vec_vec7d[i][1], vec_vec7d[i][2]));
                transform.setRotation(tf::Quaternion(vec_vec7d[i][4], vec_vec7d[i][5], vec_vec7d[i][6], vec_vec7d[i][3]));
                tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), received_frame_id_, tf_name.str()));
            }
        }

        // Publish marker array
        if (publish_marker_array_)
        {
            // 3 arrows for each coordinate system of each detected fiducial
            marker_array_size = 3*pose_array_size;
            if (marker_array_size >= prev_marker_array_size_)
            {
                marker_array_msg_.markers.resize(marker_array_size);
            }

            // publish a coordinate system from arrow markers for each object
            for (unsigned int i=0; i<pose_array_size; i++)
            {
//.........这里部分代码省略.........

 void send(const string& name,const RigidTransform& T,const char* parent="world") {
   tf::Transform transform;
   KlamptToROS(T,transform);
   broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), parent, name));
 }

void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq)
{
	processingNewCloud = true;
	BoolSetter stopProcessingSetter(processingNewCloud, false);

	// if the future has completed, use the new map
	processNewMapIfAvailable();
	
	// IMPORTANT:  We need to receive the point clouds in local coordinates (scanner or robot)
	timer t;
	
	// Convert point cloud
	const size_t goodCount(newPointCloud->features.cols());
	if (goodCount == 0)
	{
		ROS_ERROR("I found no good points in the cloud [at mapper.cpp]");
		return;
	}
	
	// Dimension of the point cloud, important since we handle 2D and 3D
	const int dimp1(newPointCloud->features.rows());

	ROS_INFO("Processing new point cloud");
	{
		timer t; // Print how long take the algo
		
		// Apply filters to incoming cloud, in scanner coordinates
		inputFilters.apply(*newPointCloud);
		
		ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]");
	}
	
	string reason;
	// Initialize the transformation to identity if empty
 	if(!icp.hasMap())
 	{
		// we need to know the dimensionality of the point cloud to initialize properly
		publishLock.lock();
		TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1);
		publishLock.unlock();
	}

	// Fetch transformation from scanner to odom
	// Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix()
	PM::TransformationParameters TOdomToScanner;
	try
	{
		TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>(
				PointMatcher_ros::transformListenerToEigenMatrix<float>(
				tfListener,
				scannerFrame,
				odomFrame,
				stamp
			), dimp1);
	}
	catch(tf::ExtrapolationException e)
	{
		ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp);
		return;
	}

	//Correct TOdomToMap to fit dimp1 (dimension of input data)
	//We need to do this because we could have called 'CorrectPose' which always returns a [4][4] matrix
	TOdomToMap = PointMatcher_ros::eigenMatrixToDim<float>(TOdomToMap, dimp1);

	ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner);
	ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap);
		
 	const PM::TransformationParameters TscannerToMap = transformation->correctParameters(TOdomToMap * TOdomToScanner.inverse());
	ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap);
	
	// Ensure a minimum amount of point after filtering
	const int ptsCount = newPointCloud->features.cols();
	if(ptsCount < minReadingPointCount)
	{
		ROS_ERROR_STREAM("Not enough points in newPointCloud: only " << ptsCount << " pts.");
		return;
	}

	// Initialize the map if empty
 	if(!icp.hasMap())
 	{
		ROS_INFO_STREAM("Creating an initial map");
		mapCreationTime = stamp;
		setMap(updateMap(newPointCloud.release(), TscannerToMap, false));
		// we must not delete newPointCloud because we just stored it in the mapPointCloud
		return;
	}
	
	// Check dimension
	if (newPointCloud->features.rows() != icp.getInternalMap().features.rows())
	{
		ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1);
		return;
	}

	// Call this function in order to verbose errors when matching the newPointCloud and the icp Internal Map
	debugFeaturesDescriptors(*newPointCloud);

	try 
//.........这里部分代码省略.........

void RosAriaNode::publish()
{
  // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
  pos = robot->getPose();
  tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
    pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
  position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s.
  position.twist.twist.linear.y = robot->getLatVel()/1000.0;
  position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
  
  position.header.frame_id = frame_id_odom;
  position.child_frame_id = frame_id_base_link;
  position.header.stamp = ros::Time::now();
  pose_pub.publish(position);

  ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", 
    position.header.stamp.toSec(), 
    (double)position.pose.pose.position.x,
    (double)position.pose.pose.position.y,
    (double)position.pose.pose.orientation.w,
    (double) position.twist.twist.linear.x,
    (double) position.twist.twist.linear.y,
    (double) position.twist.twist.angular.z
  );


  // publishing transform odom->base_link
  odom_trans.header.stamp = ros::Time::now();
  odom_trans.header.frame_id = frame_id_odom;
  odom_trans.child_frame_id = frame_id_base_link;
  
  odom_trans.transform.translation.x = pos.getX()/1000;
  odom_trans.transform.translation.y = pos.getY()/1000;
  odom_trans.transform.translation.z = 0.0;
  odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
  
  odom_broadcaster.sendTransform(odom_trans);
  
  // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
  int stall = robot->getStallValue();
  unsigned char front_bumpers = (unsigned char)(stall >> 8);
  unsigned char rear_bumpers = (unsigned char)(stall);

  bumpers.header.frame_id = frame_id_bumper;
  bumpers.header.stamp = ros::Time::now();

  std::stringstream bumper_info(std::stringstream::out);
  // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
  for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
  {
    bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
    bumper_info << " " << (front_bumpers & (1 << (i+1)));
  }
  ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());

  bumper_info.str("");
  // Rear bumpers have reverse order (rightmost is LSB)
  unsigned int numRearBumpers = robot->getNumRearBumpers();
  for (unsigned int i=0; i<numRearBumpers; i++)
  {
    bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
    bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
  }
  ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
  
  bumpers_pub.publish(bumpers);

  //Publish battery information
  // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
  std_msgs::Float64 batteryVoltage;
  batteryVoltage.data = robot->getRealBatteryVoltageNow();
  voltage_pub.publish(batteryVoltage);

  if(robot->haveStateOfCharge())
  {
    std_msgs::Float32 soc;
    soc.data = robot->getStateOfCharge()/100.0;
    state_of_charge_pub.publish(soc);
  }

  // publish recharge state if changed
  char s = robot->getChargeState();
  if(s != recharge_state.data)
  {
    ROS_INFO("RosAria: publishing new recharge state %d.", s);
    recharge_state.data = s;
    recharge_state_pub.publish(recharge_state);
  }

  // publish motors state if changed
  bool e = robot->areMotorsEnabled();
  if(e != motors_state.data || !published_motors_state)
  {
	ROS_INFO("RosAria: publishing new motors state %d.", e);
	motors_state.data = e;
	motors_state_pub.publish(motors_state);
	published_motors_state = true;
  }

  // Publish sonar information, if enabled.
//.........这里部分代码省略.........