C++ ArTime::getSec方法代码示例

int Search::seek(int lastPositionXY[2],int currentPositionXY[2])
{
	Movement move = Movement(myRobot);
	double range = mySonar->currentReadingPolar(-70, 70) - myRobot->getRobotRadius();

	ArTime start;
        //using timeSet to start the counting the first time we enter Search
        if (timeSet == 0)
                timeSet = start.getSec();

        //if searching for more thatn 1o secs switch to goTo
        if(start.getSec() - timeSet > 10)
        {	timeSet = 0;
		return 3;
	}

        //if we have found a target
        if( currentPositionXY[0] != -1 )
        {       timeSet = 0;
                std::cout << "Found target " << currentPositionXY[0] << std::endl;
                return 1;
        }

        // if we are safe
        if(range > stopDistance)
        {       //if the target was last seen on the right
                if( lastPositionXY[0] > width/2 )
                        move.turn(-36);
                //if the target was last seen on the left
                if( lastPositionXY[0] =< width/2 )
                        move.turn(36);
                return 2;
        }

	//if we are not safe
	else
	{	move.stop();

int main(void)
{
  Aria::init();
  ArLog::init(ArLog::StdOut, ArLog::Normal, "", true);

  if (ArTime::usingMonotonicClock())
    ArLog::log(ArLog::Normal, "Using monotonic clock");
  else
    ArLog::log(ArLog::Normal, "Using normal clock that will have issues if time of day is changed");

  ArTime time;
  while (Aria::getRunning())
  {
    time.setToNow();
    ArLog::log(ArLog::Normal, "%10s\t%lu%10s\t%lu", "", time.getSec(), "",
	       time.getMSec());
    ArUtil::sleep(1000);
  }
}

void RosArnlNode::publish()
{
  // todo could only publish if robot not stopped (unless arnl has TriggerTime
  // set in which case it might update localization even ifnot moving), or
  // use a callback from arnl for robot pose updates rather than every aria
  // cycle.  In particular, getting the covariance is a bit computational
  // intensive and not everyone needs it.

  ArTime tasktime;

  // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
  ArPose pos = arnl.robot->getPose();

  // convert mm and degrees to position meters and quaternion angle in ros pose
  tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
    pos.getY()/1000, 0)), pose_msg.pose.pose); 

  pose_msg.header.frame_id = "map";

  // ARIA/ARNL times are in reference to an arbitrary starting time, not OS
  // clock, so find the time elapsed between now and last ARNL localization
  // to adjust the time stamp in ROS time vs. now accordingly.
  //pose_msg.header.stamp = ros::Time::now();
  ArTime loctime = arnl.locTask->getLastLocaTime();
  ArTime arianow;
  const double dtsec = (double) loctime.mSecSince(arianow) / 1000.0;
  //printf("localization was %f seconds ago\n", dtsec);
  pose_msg.header.stamp = ros::Time(ros::Time::now().toSec() - dtsec);

  // TODO if robot is stopped, ARNL won't re-localize (unless TriggerTime option is
  // configured), so should we just use Time::now() in that case? or do users
  // expect long ages for poses if robot stopped?

#if 0
  {
    printf("ros now is   %12d sec + %12d nsec = %f seconds\n", ros::Time::now().sec, ros::Time::now().nsec, ros::Time::now().toSec());
    ArTime t;
    printf("aria now is  %12lu sec + %12lu ms\n", t.getSec(), t.getMSec());
    printf("arnl loc is  %12lu sec + %12lu ms\n", loctime.getSec(), loctime.getMSec());
    printf("pose stamp:= %12d sec + %12d nsec = %f seconds\n", pose_msg.header.stamp.sec, pose_msg.header.stamp.nsec, pose_msg.header.stamp.toSec());
    double d = ros::Time::now().toSec() - pose_msg.header.stamp.toSec();
    printf("diff is  %12f sec, \n", d);
    puts("----");
  }
#endif

#ifndef ROS_ARNL_NO_COVARIANCE
  ArMatrix var;
  ArPose meanp;
  if(arnl.locTask->findLocalizationMeanVar(meanp, var))
  {
    // ROS pose covariance is 6x6 with position and orientation in 3
    // dimensions each x, y, z, roll, pitch, yaw (but placed all in one 1-d
    // boost::array container)
    //
    // ARNL has x, y, yaw (aka theta):
    //    0     1     2
    // 0  x*x   x*y   x*yaw
    // 1  y*x   y*y   y*yaw
    // 2  yaw*x yaw*y yaw*yaw
    //
    // Also convert mm to m and degrees to radians.
    //
    // all elements in pose_msg.pose.covariance were initialized to -1 (invalid
    // marker) in the RosArnlNode constructor, so just update elements that
    // contain x, y and yaw.
  
    pose_msg.pose.covariance[6*0 + 0] = var(0,0)/1000.0;  // x/x
    pose_msg.pose.covariance[6*0 + 1] = var(0,1)/1000.0;  // x/y
    pose_msg.pose.covariance[6*0 + 5] = ArMath::degToRad(var(0,2)/1000.0);    //x/yaw
    pose_msg.pose.covariance[6*1 + 0] = var(1,0)/1000.0;  //y/x
    pose_msg.pose.covariance[6*1 + 1] = var(1,1)/1000.0;  // y/y
    pose_msg.pose.covariance[6*1 + 5] = ArMath::degToRad(var(1,2)/1000.0);  // y/yaw
    pose_msg.pose.covariance[6*5 + 0] = ArMath::degToRad(var(2,0)/1000.0);  //yaw/x
    pose_msg.pose.covariance[6*5 + 1] = ArMath::degToRad(var(2,1)/1000.0);  // yaw*y
    pose_msg.pose.covariance[6*5 + 5] = ArMath::degToRad(var(2,2)); // yaw*yaw
  }
#endif
  
  pose_pub.publish(pose_msg);


  if(action_executing) 
  {
    move_base_msgs::MoveBaseFeedback feedback;
    feedback.base_position.pose = pose_msg.pose.pose;
    actionServer.publishFeedback(feedback);
  }


  // publishing transform map->base_link
  map_trans.header.stamp = ros::Time::now();
  map_trans.header.frame_id = frame_id_map;
  map_trans.child_frame_id = frame_id_base_link;
  
  map_trans.transform.translation.x = pos.getX()/1000;
  map_trans.transform.translation.y = pos.getY()/1000;
  map_trans.transform.translation.z = 0.0;
  map_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);

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