C++ ArPose类代码示例

void sendPoseRobot(ArServerClient* client, ArNetPacket* packet) {
	ArNetPacket reply;
	ArPose pose = gotoGoal.getPose();
	reply.doubleToBuf(pose.getX());
	reply.doubleToBuf(pose.getY());
    client->sendPacketUdp(&reply);
}

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

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

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

	// Encoders:
	out_left_encoder_ticks	= THE_ROBOT->getLeftEncoder();
	out_right_encoder_ticks	= THE_ROBOT->getRightEncoder();

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

/*!
 * Callback function for the l key. Localizes the robot at the home position.
 * Warning: Robot must be physically at the first home pose.
 */
void
lkeyCB(void)
{
  roundRobinFlag = false;

  static int once = 0;
  ArPose pose;
  if(!once && advancedptr->myHomeList.size()>0){
    once = 1;
    ArPose top = advancedptr->myHomeList.front()->getPose();
    printf("Localizing at %s %5.2f %5.2f %5.2f\n",
       advancedptr->myHomeList.front()->getName(),
       top.getX(), top.getY(), top.getTh());
    //
    // Set the pose to localize the robot about.
    //
    pose = top;
  } else {
    advancedptr->myRobot->lock();
    pose = advancedptr->myRobot->getPose();
    advancedptr->myRobot->unlock();
  }
  //
  // Do the localization at the current pose.
  //
  if(advancedptr->localizeAtSetPose(pose)){
    printf("Localized at current pose: Score: %5.2f out of 1.0\n",
       advancedptr->getLocalizationScore());
  }else {
    printf("Failed to localize\n");
  }
}

/*!
 * Callback function for the r key. Gets the robot to pick the next
 * goal point and plan to it. and keep going around the list.
 */
void
rkeyCB(void)
{
  if(roundRobinFlag == false){
    roundRobinFlag = true;
  }else{
    roundRobinFlag = false;
    return;
  }

  if(advancedptr->myLocaTask->getInitializedFlag()){
    if(advancedptr->myGoalList.size()>0){
      ArMapObject* front = advancedptr->myGoalList.front();
      ArPose top = front->getPose();
      bool headingFlag = false;
      if(strcasecmp(front->getType(), "GoalWithHeading") == 0)
    headingFlag = true;
      else
    headingFlag = false;
      printf("Path planing to goal %s %5.2f %5.2f %5.2f: %d poses\n",
         front->getName(),
         top.getX(), top.getY(), top.getTh(),
         advancedptr->myGoalList.size());
      advancedptr->myGoalList.pop_front();
      advancedptr->myGoalList.push_back(front);
      //
      // Setup the pathplanning task to this destination.
      //
      advancedptr->pathPlanFromCurrentToDest(top, headingFlag);
    }
  }else{
    printf("Localize the robot first\n");
  }
}

/**
 * @brief read the robot's position and publish them to topic DEFAULT_TOPIC_ODOM
 */
void AriaCore::publish_pose()
{
    ArPose  pos;

    m_robot.lock();
    {
        pos = m_robot.getPose();
    }
    m_robot.unlock();

    // set position
    nav_msgs::Odometry msg;
    msg.header.stamp    = ros::Time::now();
    msg.header.frame_id = m_frameIDOdom;
    msg.child_frame_id  = m_frameIDRobot;

    tf::poseTFToMsg( tf::Pose(tf::Quaternion(0, 0, pos.getTh()*D2R),
                tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), msg.pose.pose);

    // set vel
    m_robot.lock();
    {
        msg.twist.twist.linear.x = m_robot.getVel()/1000.0;
        msg.twist.twist.angular.z= m_robot.getRotVel()*D2R;
    }
    m_robot.unlock();

    m_pubPos.publish(msg);
}

/*-------------------------------------------------------------
					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
	THROW_EXCEPTION("MRPT has been compiled with 'MRPT_BUILD_ARIA'=OFF, so this class cannot be used.");
#endif
}

/**
   @param pose the coord system from which we transform to abs world coords
*/
AREXPORT void ArTransform::setTransform(ArPose pose) 
{ 
  myTh = pose.getTh();
  myCos = ArMath::cos(-myTh);
  mySin = ArMath::sin(-myTh);
  myX = pose.getX();
  myY = pose.getY();
}

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

int main (int argc, char** argv) {
	
	Aria::init();
	Arnl::init();
	ArRobot robot;
	ArArgumentParser parser(&argc, argv);
	parser.loadDefaultArguments();
	ArRobotConnector robotConnector(&parser, &robot);
	//ArAnalogGyro gyro = new 
	ArAnalogGyro gyro(&robot);
	if (!robotConnector.connectRobot()) {
		ArLog::log(ArLog::Terse, "Could not connect to the robot.");
		if(parser.checkHelpAndWarnUnparsed())
		{
			// -help not given, just exit.
			Aria::logOptions();
			Aria::exit(1);
			return 1;
		}
	}
	
	ArSonarDevice sonarDev;
	robot.addRangeDevice(&sonarDev);
	
	robot.runAsync(true);
	
	ArMap map("office.map");
	// set it up to ignore empty file names (otherwise if a configuration omits
	// the map file, the whole configuration change will fail)
	map.setIgnoreEmptyFileName(true);
	// ignore the case, so that if someone is using MobileEyes or
	// MobilePlanner from Windows and changes the case on a map name,
	// it will still work.
	map.setIgnoreCase(true);
	
	ArSonarLocalizationTask locTask(&robot, &sonarDev, &map);
	
	locTask.localizeRobotAtHomeBlocking();
	
	robot.runAsync(true);
	robot.enableMotors();
	//robot.lock();
	robot.setVel(200);
	//robot.unlock();
	ArPose pose;
	locTask.forceUpdatePose(pose);
	while(true) {
	//while (robot.blockingConnect()){
		//robot.lock();
		//ArPose pose  = robot.getPose();
		//pose.setX(100);
		//robot.moveTo(pose);
		//t = robot.getLastOdometryTime();
		//int a = interp.getPose(t, &pose);
		ArLog::log(ArLog::Normal, "x = %f \t y = %f\n", pose.getX(), pose.getY());
		//robot.unlock();
	} 
}

geometry_msgs::Pose arPoseToRosPose(const ArPose& arpose)
{
  // TODO use tf::poseTFToMsg instead?
  geometry_msgs::Pose rospose;
  rospose.position.x = arpose.getX() / 1000.0;
  rospose.position.y = arpose.getY() / 1000.0;
  rospose.position.z = 0;
  tf::quaternionTFToMsg(tf::createQuaternionFromYaw(arpose.getTh()*M_PI/180.0), rospose.orientation);
  return rospose;
}

void testNotPerp(ArLineSegment *segment, ArPose perp, char *name)
{
  ArPose pose;
  if (segment->getPerpPoint(perp, &pose))
  {
    printf("%s was perp but shouldn't have been, at %.0f %.0f\n", name,
	   pose.getX(), pose.getY());
    exit(1);
  }
}

AREXPORT ArMapObject::ArMapObject(const char *type, 
					                        ArPose pose, 
                                  const char *description,
 		                              const char *iconName, 
                                  const char *name,
 		                              bool hasFromTo, 
                                  ArPose fromPose, 
                                  ArPose toPose) :
  myType((type != NULL) ? type : ""),
  myBaseType(),
  myName((name != NULL) ? name : "" ),
  myDescription((description != NULL) ? description : "" ),
  myPose(pose),
  myIconName((iconName != NULL) ? iconName : "" ),
  myHasFromTo(hasFromTo),
  myFromPose(fromPose),
  myToPose(toPose),
  myFromToSegments(),
  myStringRepresentation()
{
  if (myHasFromTo)
  {
    double angle = myPose.getTh();
    double sa = ArMath::sin(angle);
    double ca = ArMath::cos(angle);
    double fx = fromPose.getX();
    double fy = fromPose.getY();
    double tx = toPose.getX();
    double ty = toPose.getY();
    ArPose P0((fx*ca - fy*sa), (fx*sa + fy*ca));
    ArPose P1((tx*ca - fy*sa), (tx*sa + fy*ca));
    ArPose P2((tx*ca - ty*sa), (tx*sa + ty*ca));
    ArPose P3((fx*ca - ty*sa), (fx*sa + ty*ca));
    myFromToSegments.push_back(ArLineSegment(P0, P1));
    myFromToSegments.push_back(ArLineSegment(P1, P2));
    myFromToSegments.push_back(ArLineSegment(P2, P3));
    myFromToSegments.push_back(ArLineSegment(P3, P0));
    
    myFromToSegment.newEndPoints(fromPose, toPose);
  }
  else { // pose
    size_t whPos = myType.rfind("WithHeading");
    size_t whLen = 11;
    if (whPos > 0) {
      if (whPos == myType.size() - whLen) {
        myBaseType = myType.substr(0, whPos);
      }
    }
  } // end else pose

  IFDEBUG(
  ArLog::log(ArLog::Normal, 
             "ArMapObject::ctor() created %s (%s)",
             myName.c_str(), myType.c_str());
  );

AREXPORT void ArActionGotoStraight::setGoalRel(double dist, 
					       double deltaHeading,
					       bool backToGoal, 
					       bool justDistance)
{
  ArPose goal;
  goal.setX(dist * ArMath::cos(deltaHeading));
  goal.setY(dist * ArMath::sin(deltaHeading));
  goal = myRobot->getToGlobalTransform().doTransform(goal);
  setGoal(goal, backToGoal, justDistance);
}

void testPerp(ArLineSegment *segment, ArPose perp, ArPose perpPoint, 
	      char *name)
{
  ArPose pose;
  if (!segment->getPerpPoint(perp, &pose) || 
      fabs(pose.getX() - perpPoint.getX()) > .001 ||
      fabs(pose.getY() - perpPoint.getY()) > .001)
  {
    printf("%s wasn't perp but should have been\n", name);
    exit(1);
  }
}

/*!
 * Interact with user on the terminal.
 */
void
interact()
{
  ArMap* ariamap = advancedptr->myMap;
  sleep(1);
  advancedptr->getAllGoals(ariamap);
  advancedptr->getAllRobotHomes(ariamap);

  /// MPL
//  lkeyCB();
  advancedptr->myLocaTask->localizeRobotAtHomeNonBlocking();
  //
  // Interact with user using keyboard.
  //
  ArGlobalFunctor lCB(&lkeyCB);
  ArGlobalFunctor pCB(&pkeyCB);
  ArGlobalFunctor hCB(&hkeyCB);
  ArGlobalFunctor rCB(&rkeyCB);
  ArGlobalFunctor qCB(&quitCB);
  ArGlobalFunctor escapeCB(&quitCB);

  keyHandler.addKeyHandler('l', &lCB);
  keyHandler.addKeyHandler('p', &pCB);
  keyHandler.addKeyHandler('h', &hCB);
  keyHandler.addKeyHandler('r', &rCB);
  keyHandler.addKeyHandler('q', &qCB);
  keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);

  printf("Put robot at RobotHome and press 'l' to localize first.\n\   
Press 'p' to move to the next goal\n\
Press 'h' to move to the first home\n\
Press 'r' to move to the goals in order\n\
Press 'q' to quit\n");   
  while (advancedptr->myLocaTask->getRunning() &&
     advancedptr->myPathPlanningTask->getRunning()){

    keyHandler.checkKeys();
    ArUtil::sleep(250);

    advancedptr->myRobot->lock();
    ArPose rpose = advancedptr->myRobot->getPose();
    double lvel = advancedptr->myRobot->getVel();
    double avel = advancedptr->myRobot->getRotVel();
    double volts = advancedptr->myRobot->getBatteryVoltage();
    advancedptr->myRobot->unlock();
    if(advancedptr->myLocaTask->getInitializedFlag()){
      printf("\r%5.2f %5.2f %5.2f: %5.2f %5.2f: %4.1f\r",
         rpose.getX(), rpose.getY(), rpose.getTh(), lvel, avel, volts);
      fflush(stdout);
    }
  }
}