C++ ArPose::getX方法代码示例

void addGoalFinished(ArPose pose) {
	ArLog::log(ArLog::Normal, "Finished at x = %f, y = %f", pose.getX(), pose.getY());
}

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; //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.
//.........这里部分代码省略.........

int main(int argc, char **argv)
{

  Aria::init();
  ArLog::init(ArLog::StdOut, ArLog::Verbose);

  ArMap testMap;

  ArTime timer;

  ArGlobalFunctor mapChangedCB(&mapChanged);
  testMap.addMapChangedCB(&mapChangedCB);


  if (argc <= 1)
  {
    printf("mapTest: Usage %s <map> <map2:optional>\n", argv[0]);
    Aria::exit(1);
  }

  timer.setToNow();
  if (!testMap.readFile(argv[1]))
  {
    printf("mapTest: Could not read map '%s'\n", argv[1]);
    Aria::exit(2);
  }
  printf("mapTest: Took %ld ms to read file\n", timer.mSecSince());
/*
  printf("mapTest: ChangeTimes (in ms): mapObjects %ld points %ld mapInfo %ld\n",
	 testMap.getMapObjectsChanged().mSecSince(),
	 testMap.getPointsChanged().mSecSince(),
	 testMap.getMapInfoChanged().mSecSince());
*/
  timer.setToNow();
  if(!testMap.writeFile("mapTest.map"))
  {
    printf("mapTest: Error could not write new map to mapTest.map");
    Aria::exit(3);
  }
  printf("mapTest: Took %ld ms to write file mapTest.map\n", timer.mSecSince());

  std::list<ArMapObject *>::iterator objIt;
  ArMapObject *obj;
  for (objIt = testMap.getMapObjects()->begin(); 
       objIt != testMap.getMapObjects()->end(); 
       objIt++)
  {
    obj = (*objIt);
    printf("mapTest: Map object: %s named \"%s\". Pose: %0.2f,%0.2f,%0.2f. ", obj->getType(), obj->getName(), obj->getPose().getX(), obj->getPose().getY(), obj->getPose().getTh());
    if(obj->hasFromTo())
      printf("mapTest: Extents: From %0.2f,%0.2f to %0.2f,%0.2f.", obj->getFromPose().getX(), obj->getFromPose().getY(), obj->getToPose().getX(), obj->getToPose().getY());
    printf("mapTest: \n");

/*
    if (strcasecmp(obj->getType(), "Goal") == 0 ||
	strcasecmp(obj->getType(), "GoalWithHeading") == 0)
    {
      printf("mapTest: Map object: Goal %s\n", obj->getName());
    }
    else if (strcasecmp(obj->getType(), "ForbiddenLine") == 0 &&
	obj->hasFromTo())
    {
      printf("mapTest: Map object: Forbidden line from %.0f %.0f to %.0f %.0f\n",
	     obj->getFromPose().getX(), obj->getFromPose().getY(),
	     obj->getToPose().getX(), obj->getToPose().getY());
    }

*/

  }

  std::list<ArArgumentBuilder*>* objInfo = testMap.getMapInfo();
  for(std::list<ArArgumentBuilder*>::const_iterator i = objInfo->begin();
      i != objInfo->end();
      i++)
  {
    printf("mapTest: MapInfo object definition:\n----\n");
    (*i)->log();
    printf("mapTest: ----\n");
  }

  printf("mapTest: First 5 data points:\n");
  std::vector<ArPose>::iterator pIt;
  ArPose pose;
  int n = 0;
  for (pIt = testMap.getPoints()->begin(); 
       pIt != testMap.getPoints()->end(); 
       pIt++)
  {
    pose = (*pIt);
    if (n > 5)
      exit(0);
    printf("mapTest: \t%.0f %.0f\n", pose.getX(), pose.getY());
    n++;
    // the points are gone through
  }

  if (argc >= 3)
  {
    timer.setToNow();
//.........这里部分代码省略.........

/*-------------------------------------------------------------
					getOdometryIncrement
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometryIncrement(
	poses::CPose2D	&out_incr_odom,
	double			&out_lin_vel,
	double			&out_ang_vel,
	int64_t			&out_incr_left_encoder_ticks,
	int64_t			&out_incr_right_encoder_ticks
	)
{
#if MRPT_HAS_ARIA
	ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
	THE_ROBOT->lock();

	static CPose2D	last_pose;
	static int64_t  last_left_ticks=0, last_right_ticks=0;

	CPose2D		cur_pose;
	int64_t		cur_left_ticks, cur_right_ticks;

	// Velocities:
	out_lin_vel = THE_ROBOT->getVel() * 0.001;
	out_ang_vel = DEG2RAD( THE_ROBOT->getRotVel() );

	// Current odometry:
	ArPose	pose = THE_ROBOT->getEncoderPose();
	cur_pose.x( pose.getX() * 0.001 );
	cur_pose.y( pose.getY() * 0.001 );
	cur_pose.phi( DEG2RAD( pose.getTh() ) );

	// Current encoders:
	cur_left_ticks	= THE_ROBOT->getLeftEncoder();
	cur_right_ticks	= THE_ROBOT->getRightEncoder();

	// Compute increment:
	if (m_firstIncreOdometry)
	{
		// First time:
		m_firstIncreOdometry = false;
		out_incr_odom = CPose2D(0,0,0);
		out_incr_left_encoder_ticks  = 0;
		out_incr_right_encoder_ticks = 0;
	}
	else
	{
		// Normal case:
		out_incr_odom = cur_pose - last_pose;
		out_incr_left_encoder_ticks = cur_left_ticks - last_left_ticks;
		out_incr_right_encoder_ticks = cur_right_ticks - last_right_ticks;
	}

	// save for the next time:
	last_pose		 = cur_pose;
	last_left_ticks  = cur_left_ticks;
	last_right_ticks = cur_right_ticks;

	THE_ROBOT->unlock();
#else
	MRPT_UNUSED_PARAM(out_incr_odom); MRPT_UNUSED_PARAM(out_lin_vel); MRPT_UNUSED_PARAM(out_ang_vel);
	MRPT_UNUSED_PARAM(out_incr_left_encoder_ticks); MRPT_UNUSED_PARAM(out_incr_right_encoder_ticks);
	THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}

AREXPORT void ArForbiddenRangeDevice::processReadings(void)
{
  ArPose intersection;
  std::list<ArLineSegment *>::iterator it;
  
  lockDevice();
  myDataMutex.lock();

  myCurrentBuffer.beginRedoBuffer();

  if (!myIsEnabled)
  {
    myCurrentBuffer.endRedoBuffer();
    myDataMutex.unlock();
    unlockDevice();
    return;
  }

  ArLineSegment *segment;
  ArPose start;
  double startX;
  double startY;
  ArPose end;
  double angle;
  double length;
  double gone;
  double sin;
  double cos;
  double atX;
  double atY;
  double robotX = myRobot->getX();
  double robotY = myRobot->getY();
  double max = (double) myMaxRange;
  double maxSquared = (double) myMaxRange * (double) myMaxRange;
  ArTime startingTime;
  //startingTime.setToNow();
  // now see if the end points of the segments are too close to us
  for (it = mySegments.begin(); it != mySegments.end(); it++)
  {
    segment = (*it);
    // if either end point or some perpindicular point is close to us
    // add the line's data
    if (ArMath::squaredDistanceBetween(
	    segment->getX1(), segment->getY1(), 
	    myRobot->getX(), myRobot->getY()) < maxSquared ||
	ArMath::squaredDistanceBetween(
		segment->getX2(), segment->getY2(), 
		myRobot->getX(), myRobot->getY()) < maxSquared ||
	segment->getPerpDist(myRobot->getPose()) < max)
    {
      start.setPose(segment->getX1(), segment->getY1());
      end.setPose(segment->getX2(), segment->getY2());
      angle = start.findAngleTo(end);
      cos = ArMath::cos(angle);
      sin = ArMath::sin(angle);
      startX = start.getX();
      startY = start.getY();
      length = start.findDistanceTo(end);
      // first put in the start point if we should
      if (ArMath::squaredDistanceBetween(
	      startX, startY, robotX, robotY) < maxSquared)
	myCurrentBuffer.redoReading(start.getX(), start.getY());
      // now walk the length of the line and see if we should put the points in
      for (gone = 0; gone < length; gone += myDistanceIncrement)
      {
	atX = startX + gone * cos;
	atY = startY + gone * sin;
	if (ArMath::squaredDistanceBetween(
		atX, atY, robotX, robotY) < maxSquared)
	  myCurrentBuffer.redoReading(atX, atY);
      }
      // now check the end point
      if (end.squaredFindDistanceTo(myRobot->getPose()) < maxSquared)
	myCurrentBuffer.redoReading(end.getX(), end.getY());
    }
  }
  myDataMutex.unlock();
  // and we're done
  myCurrentBuffer.endRedoBuffer();
  unlockDevice();
  //printf("%d\n", startingTime.mSecSince());
}

void addGoalFailed(ArPose pose){
	ArLog::log(ArLog::Normal, "Fail at x = %f, y = %f", pose.getX(), pose.getY());
}

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

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

void GPSMapTools::addHomeHere(ArArgumentBuilder *args)
{
  if(!checkGPS("adding home")) return;
  if(!checkMap("adding home")) return;
  ArPose p = getCurrentPosFromGPS();
  ArLog::log(ArLog::Normal, "GPSMapTools: Adding home in map at GPS position (x=%.2f, y=%.2f)",  p.getX(), p.getY());
  myMap->lock();
  ArMapObject *newobj;
  myMap->getMapObjects()->push_back(newobj = new ArMapObject("Home", p, "Home", "ICON", args->getFullString(), false, ArPose(0, 0), ArPose(0, 0)));
  printf("\tnew map object is:\n\t%s\n", newobj->toString());
  newobj->log();
  myMap->writeFile(myMap->getFileName(), true);
  myMap->unlock();
  reloadMapFile();
}

void PRM::render()
{
    //Desenhando o mapa em si
    glBegin(GL_QUADS);

    for(int x=0;x<mMapSize;x++)
    {
        for(int y=0;y<mMapSize;y++)
        {
            if(mMap[x][y] != 1.0)
            {
                int value = 255*mMap[x][y];
                drawBox(
                            (x-1-mMapSize/2)*celRange,
                            (mMapSize/2 - y+1)*celRange,
                            celRange,
                            celRange,
                            QColor(value,value,value)
                            );
            }
        }
    }
    glEnd();

    if(mGraph)
    {
        //Desenhando os Arcos
        glColor3f(0.0f,0.0f,1.0f);
        glBegin(GL_LINES);

        for(int i =0; i<mGraph->E.size();i++)
        {
            ArPose *pL = mGraph->E.at(i)->mNodeL->mPose, *pR = mGraph->E.at(i)->mNodeR->mPose;
            glVertex2f((pL->getX()-mMapSize/2)*celRange,(mMapSize/2 - pL->getY())*celRange);
            glVertex2f((pR->getX()-mMapSize/2)*celRange,(mMapSize/2 - pR->getY())*celRange);
        }
        glEnd();

        //Desenhando os  nodos
        glBegin(GL_QUADS);
        for(int i =0;i<mGraph->V.size();i++)
        {
            ArPose *p = mGraph->V.at(i)->mPose;

            drawBox(
                (p->getX() - 1 - mMapSize/2)*celRange,
                (mMapSize/2 - 1 - p->getY())*celRange,
                celRange*2,
                celRange*2,
                QColor(0,255,255)
                );
        }
        glEnd();
    }


    if(mInitNode && mGoalNode && path.size())
    {
        //Desenhando os Arcos
        glColor3f(1.0f,0.0f,1.0f);
        glLineWidth(3.0f);
        glBegin(GL_LINES);

        for(int i =0; i<path.size();i++)
        {
            ArPose *pL = path.at(i)->mNodeL->mPose, *pR = path.at(i)->mNodeR->mPose;
            glVertex2f((pL->getX()-mMapSize/2)*celRange,(mMapSize/2 - pL->getY())*celRange);
            glVertex2f((pR->getX()-mMapSize/2)*celRange,(mMapSize/2 - pR->getY())*celRange);
        }
        glEnd();
        glLineWidth(1.0f);
    }

    //Desenhando os objetivo e posicao inicial
    if(mInitNode)
    {
        glBegin(GL_QUADS);
        drawBox(
            (mInitNode->mPose->getX() - 2 - mMapSize/2)*celRange,
            (mMapSize/2 - 1 - mInitNode->mPose->getY())*celRange,
            celRange*3,
            celRange*3,
            QColor(0,150,0)
            );
        glEnd();
    }
    if(mGoalNode)
    {
        glBegin(GL_QUADS);
        drawBox(
            (mGoalNode->mPose->getX() - 2 - mMapSize/2)*celRange,
            (mMapSize/2 - 1 - mGoalNode->mPose->getY())*celRange,
            celRange*3,
            celRange*3,
            QColor(255,0,0)
            );
        glEnd();
    }
}

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; //Aria returns velocity in mm/s.
  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;
  }

  if (robot->areSonarsEnabled())
  {
//.........这里部分代码省略.........

/*!
 * This is the called when path to goal fails
 *
 * @param goal: Goal it was assigned.
 */
void
Advanced::goalFailed(ArPose goal)
{
  printf("goalFailed:Main: %5.2f %5.2f %5.2f\07\n",
     goal.getX(), goal.getY(), goal.getTh());
}

void RosArnlNode::execute_action_cb(const move_base_msgs::MoveBaseGoalConstPtr &goal)
{
  // the action execute callback is initiated by the first goal sent.  it should
  // continue running until the goal is reached or failed.  we need to also
  // check here for new goals received while the previous goal is in progress,
  // in which case we will set the new goal.   arnl callbacks are used to handle
  // reaching the goal, failure, or recognizing that the goal has been
  // preempted, which allows it to work in combination with MobileEyes or other
  // clients as well as the ros action client.
  ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: begin execution for new goal.");
  action_executing  = true;
  ArPose goalpose = rosPoseToArPose(goal->target_pose);
  ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: planning to goal %.0fmm, %.0fmm, %.0fdeg", goalpose.getX(), goalpose.getY(), goalpose.getTh());
  bool heading = !ArMath::isNan(goalpose.getTh());
  //arnl.pathTask->pathPlanToPose(goalpose, heading);
  arnl.modeGoto->gotoPose(goalpose, heading);
  arnl_goal_done = false;
  while(n.ok())
  {
    // TODO check for localization lost

    if(arnl_goal_done)
    {
      ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: goal done, ending execution.");
      action_executing = false;
      return;
    }

    if(actionServer.isPreemptRequested())
    {
      if(actionServer.isNewGoalAvailable())
      {
        // we were preempted by a new goal
        move_base_msgs::MoveBaseGoalConstPtr newgoal = actionServer.acceptNewGoal();
        goalpose = rosPoseToArPose(newgoal->target_pose);
        ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: new goal interrupted current goal.  planning to new goal %.0fmm, %.0fmm, %.0fdeg", goalpose.getX(), goalpose.getY(), goalpose.getTh());
        bool heading = !ArMath::isNan(goalpose.getTh());
        arnl.modeGoto->gotoPose(goalpose, heading);
      
        // action server will be set to preempted state by arnl interrupt
        // callback.
      }
      else
      {
        // we were simply asked to just go to "preempted" end state, with no new
        // goal
        ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: forced to preempted, ending execution.");
        actionServer.setPreempted();
        action_executing = false;
        arnl.modeGoto->deactivate();
        
        return;
      }
    }

    // feedback is published in the publish() task callback 

  }
  // node is shutting down, n.ok() returned false
  ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: action: node shutting down, setting aborted state and ending execution.");
  actionServer.setAborted(move_base_msgs::MoveBaseResult(), "Setting aborted state since node is shutting down.");
  action_executing = false;
}

void RosArnlNode::simple_goal_sub_cb(const geometry_msgs::PoseStampedConstPtr &msg)
{
  ArPose p = rosPoseToArPose(msg->pose);
  bool heading = !ArMath::isNan(p.getTh());
  ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Received goal %.0fmm, %.0fmm, %.0fdeg", p.getX(), p.getY(), p.getTh());
  //arnl.pathTask->pathPlanToPose(p, true);
  arnl.modeGoto->gotoPose(p, heading);
}

void RosArnlNode::initialpose_sub_cb(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  ArPose p = rosPoseToArPose(msg->pose.pose);
  ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: Init localization pose received %.0fmm, %.0fmm, %.0fdeg", p.getX(), p.getY(), p.getTh());
  arnl.locTask->forceUpdatePose(p);
}

void addGoalInterrupted(ArPose pose) {
	ArLog::log(ArLog::Normal, "Interrupted at x = %f, y = %f", pose.getX(), pose.getY());
}