Beispiel #1
0
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 ) ));

      //}
    }
  }
}
    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++)
            {
                for (unsigned int j=0; j<3; j++)
                {
                    unsigned int idx = 3*i+j;
                    marker_array_msg_.markers[idx].header.frame_id = received_frame_id_;// "/" + frame_id;//"tf_name.str()";
                    marker_array_msg_.markers[idx].header.stamp = received_timestamp_;
                    marker_array_msg_.markers[idx].ns = "fiducials";
                    marker_array_msg_.markers[idx].id =  id_start_idx + idx;
                    marker_array_msg_.markers[idx].type = visualization_msgs::Marker::ARROW;
                    marker_array_msg_.markers[idx].action = visualization_msgs::Marker::ADD;
                    marker_array_msg_.markers[idx].color.a = 0.85;
                    marker_array_msg_.markers[idx].color.r = 0;
                    marker_array_msg_.markers[idx].color.g = 0;
                    marker_array_msg_.markers[idx].color.b = 0;

                    marker_array_msg_.markers[idx].points.resize(2);
                    marker_array_msg_.markers[idx].points[0].x = 0.0;
                    marker_array_msg_.markers[idx].points[0].y = 0.0;
                    marker_array_msg_.markers[idx].points[0].z = 0.0;
                    marker_array_msg_.markers[idx].points[1].x = 0.0;
                    marker_array_msg_.markers[idx].points[1].y = 0.0;
                    marker_array_msg_.markers[idx].points[1].z = 0.0;

                    if (j==0)
                    {
                        marker_array_msg_.markers[idx].points[1].x = 0.2;
                        marker_array_msg_.markers[idx].color.r = 255;
                    }
                    else if (j==1)
                    {
                        marker_array_msg_.markers[idx].points[1].y = 0.2;
                        marker_array_msg_.markers[idx].color.g = 255;
                    }
                    else if (j==2)
                    {
                        marker_array_msg_.markers[idx].points[1].z = 0.2;
                        marker_array_msg_.markers[idx].color.b = 255;
                    }

                    marker_array_msg_.markers[idx].pose.position.x = vec_vec7d[i][0];
                    marker_array_msg_.markers[idx].pose.position.y = vec_vec7d[i][1];
                    marker_array_msg_.markers[idx].pose.position.z = vec_vec7d[i][2];
                    marker_array_msg_.markers[idx].pose.orientation.x = vec_vec7d[i][4];
                    marker_array_msg_.markers[idx].pose.orientation.y = vec_vec7d[i][5];
                    marker_array_msg_.markers[idx].pose.orientation.z = vec_vec7d[i][6];
                    marker_array_msg_.markers[idx].pose.orientation.w = vec_vec7d[i][3];

                    ros::Duration one_hour = ros::Duration(1); // 1 second
                    marker_array_msg_.markers[idx].lifetime = one_hour;
                    marker_array_msg_.markers[idx].scale.x = 0.01; // shaft diameter
                    marker_array_msg_.markers[idx].scale.y = 0.015; // head diameter
                    marker_array_msg_.markers[idx].scale.z = 0; // head length 0=default
                }

                if (prev_marker_array_size_ > marker_array_size)
                {
                    for (unsigned int i = marker_array_size; i < prev_marker_array_size_; ++i)
                    {
                        marker_array_msg_.markers[i].action = visualization_msgs::Marker::DELETE;
                    }
                }
                prev_marker_array_size_ = marker_array_size;

                fiducials_marker_array_publisher_.publish(marker_array_msg_);
            }
        } // End: publish markers

        if (tags_vec.empty())
            return false;
        return true;
    }
Beispiel #3
0
// 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);
}
Beispiel #4
0
 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 
	{
		// Apply ICP
		PM::TransformationParameters Ticp;
		Ticp = icp(*newPointCloud, TscannerToMap);

		ROS_DEBUG_STREAM("Ticp:\n" << Ticp);
		
		// Ensure minimum overlap between scans
		const double estimatedOverlap = icp.errorMinimizer->getOverlap();
		ROS_INFO_STREAM("Overlap: " << estimatedOverlap);
		if (estimatedOverlap < minOverlap)
		{
			ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!");
			return;
		}
		
		// Compute tf
		publishStamp = stamp;
		publishLock.lock();
		TOdomToMap = Ticp * TOdomToScanner;
		// Publish tf
		tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp));
		publishLock.unlock();
		processingNewCloud = false;
 		
		ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap);

 		// Publish odometry
		if (odomPub.getNumSubscribers())
			odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Ticp, mapFrame, stamp));
	
		// Publish error on odometry
		if (odomErrorPub.getNumSubscribers())
			odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp));

		// Publish outliers
		if (outlierPub.getNumSubscribers())
		{
			//DP outliers = PM::extractOutliers(transformation->compute(*newPointCloud, Ticp), *mapPointCloud, 0.6);
			//outlierPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(outliers, mapFrame, mapCreationTime));
		}

		// check if news points should be added to the map
		if (
			mapping &&
			((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) &&
			#if BOOST_VERSION >= 104100
			(!mapBuildingInProgress)
			#else // BOOST_VERSION >= 104100
			true
			#endif // BOOST_VERSION >= 104100
		)
		{
			// make sure we process the last available map
			mapCreationTime = stamp;
			#if BOOST_VERSION >= 104100
			ROS_INFO("Adding new points to the map in background");
			mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true));
			mapBuildingFuture = mapBuildingTask.get_future();
			mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask)));
			mapBuildingInProgress = true;
			#else // BOOST_VERSION >= 104100
			ROS_INFO("Adding new points to the map");
			setMap(updateMap( newPointCloud.release(), Ticp, true));
			#endif // BOOST_VERSION >= 104100
		}
	}
	catch (PM::ConvergenceError error)
	{
		ROS_ERROR_STREAM("ICP failed to converge: " << error.what());
		return;
	}
	
	//Statistics about time and real-time capability
	int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec());
	ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]");
	if(realTimeRatio < 80)
		ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");
	else
		ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%");

	lastPoinCloudTime = stamp;
}
Beispiel #6
0
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.
  if (publish_sonar || publish_sonar_pointcloud2)
  {
    sensor_msgs::PointCloud cloud;	//sonar readings.
    cloud.header.stamp = position.header.stamp;	//copy time.
    // sonar sensors relative to base_link
    cloud.header.frame_id = frame_id_sonar;
  

    std::stringstream sonar_debug_info; // Log debugging info
    sonar_debug_info << "Sonar readings: ";

    for (int i = 0; i < robot->getNumSonar(); i++) {
      ArSensorReading* reading = NULL;
      reading = robot->getSonarReading(i);
      if(!reading) {
        ROS_WARN("RosAria: Did not receive a sonar reading.");
        continue;
      }
    
      // getRange() will return an integer between 0 and 5000 (5m)
      sonar_debug_info << reading->getRange() << " ";

      // local (x,y). Appears to be from the centre of the robot, since values may
      // exceed 5000. This is good, since it means we only need 1 transform.
      // x & y seem to be swapped though, i.e. if the robot is driving north
      // x is north/south and y is east/west.
      //
      //ArPose sensor = reading->getSensorPosition();  //position of sensor.
      // sonar_debug_info << "(" << reading->getLocalX() 
      //                  << ", " << reading->getLocalY()
      //                  << ") from (" << sensor.getX() << ", " 
      //                  << sensor.getY() << ") ;; " ;
    
      //add sonar readings (robot-local coordinate frame) to cloud
      geometry_msgs::Point32 p;
      p.x = reading->getLocalX() / 1000.0;
      p.y = reading->getLocalY() / 1000.0;
      p.z = 0.0;
      cloud.points.push_back(p);
    }
    ROS_DEBUG_STREAM(sonar_debug_info.str());
    
    // publish topic(s)

    if(publish_sonar_pointcloud2)
    {
      sensor_msgs::PointCloud2 cloud2;
      if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
      {
        ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
      }
      else
      {
        sonar_pointcloud2_pub.publish(cloud2);
      }
    }

    if(publish_sonar)
    {
      sonar_pub.publish(cloud);
    }
  } // end if sonar_enabled
}
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;
                laser_transform.setOrigin( tf::Vector3(0.3, 0, 0.05) );
                tf::Quaternion q;
                q.setRPY(0, 0, 0);
                laser_transform.setRotation(q);
                laser_broadcaster.sendTransform(tf::StampedTransform(laser_transform, ros::Time::now(), "base_link", "laser"));
//                geometry_msgs::Quaternion laser_quat = tf::createQuaternionMsgFromYaw(0);
//                geometry_msgs::TransformStamped laser_trans;
//                LaserData.header.stamp = ros::Time::now();
//                laser_trans.transform.translation.x = 0.3;
//                laser_trans.transform.translation.y = 0;
//                laser_trans.transform.translation.z = 0.05;
//                laser_trans.transform.rotation = laser_quat;
                //s_laser.publish(LaserData);
                m_odom.publish(odometerData);
				pComm->m_mutex_cap.Unlock();
			}
            if(FD_ISSET(hWait[2],&fdset))                    // 速度下发
			{
				geometry_msgs::Twist sendSpeed;
				pComm->GetCurSpeed(sendSpeed);
				double send_vx = sendSpeed.linear.x;
				double send_vy = sendSpeed.linear.y;
				double send_w = sendSpeed.angular.z;
				//限速
				send_vx = Utils::clip(send_vx,-1000.0,1000.0);
				send_vy = Utils::clip(send_vy,-1000.0,1000.0);
				send_w = Utils::clip(send_w,-50.0,50.0);
				MECANUM->SendVelocities(send_vx/100.0, send_vy/100.0, send_w*3.1415926/180.0);
				pComm->DoSafeSpeed();
			}
            if(FD_ISSET(hWait[3],&fdset))                    // 速度下发检查
			{
				pComm->SpeedCheck();
			}
			break;
		}
	}
	return 0;
}