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

/*!
 * Gets the list of home poses.
 *
 * @param mapdata: Pointer to the map class.
 *
 * @return list of home objects.
 *
 */
void
Advanced::getAllRobotHomes(ArMap* ariamap)
{
  myHomeList.clear();
  std::list<ArMapObject *>::iterator objIt;
  ArMapObject* obj;
  int i=0;
  for (objIt = ariamap->getMapObjects()->begin();
       objIt != ariamap->getMapObjects()->end();
       objIt++)
  {
    //
    // Get the forbidden lines and fill the occupancy grid there.
    //
    obj = (*objIt);
    if (strcasecmp(obj->getType(), "RobotHome") == 0)
    {
      myHomeList.push_back(obj);
      ArPose pose = obj->getPose();
      printf("RobotHome[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
         pose.getX(), pose.getY(), pose.getTh());
    }
    if (strcasecmp(obj->getType(), "Home") == 0)
    {
      myHomeList.push_back(obj);
      ArPose pose = obj->getPose();
      printf("Home[%d] = %s : %5.2f %5.2f %5.2f\n", i++, obj->getName(),
         pose.getX(), pose.getY(), pose.getTh());
    }
  }
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{   
   if(nrhs != 1)
       mexErrMsgIdAndTxt( "arnetc:arnetc_robot_update_handler:minrhs", "Incorrect number of input arguments. Must provide robot update handler object reference returned by arnetc_new_robot_update_handler.");
   uint64_t *p = (uint64_t*)mxGetData(prhs[0]);
   ArClientHandlerRobotUpdate *updateHandler = (ArClientHandlerRobotUpdate*) *p;
   if(nlhs < 1 || nlhs > 3)
       mexErrMsgIdAndTxt("arnetc:arnetc_robot_update_handler:minlhs", "Incorrect number of output arguments. Must assign to a vector [x y theta] or to two scalars (x, y) or to three scalars (x, y, theta).");

   updateHandler->lock();
   ArPose pose = updateHandler->getPose();
   updateHandler->unlock();
   
   if(nlhs == 1)
   {
       plhs[0] = mxCreateDoubleMatrix(1, 3, mxREAL);
       double* p = mxGetPr(plhs[0]);
       p[0] = pose.getX();
       p[1] = pose.getY();
       p[2] = pose.getTh();
       return;
   }
   
   if(nlhs >= 2)
   {
         plhs[0] = mxCreateDoubleScalar(pose.getX());
         plhs[1] = mxCreateDoubleScalar(pose.getY());
         if(nlhs > 2)
             plhs[2] = mxCreateDoubleScalar(pose.getTh());
   }
}

/**
   @param pose1 transform this into pose2
   @param pose2 transform pose1 into this
*/
AREXPORT void ArTransform::setTransform(ArPose pose1, ArPose pose2)
{
  myTh = ArMath::subAngle(pose2.getTh(), pose1.getTh());
  myCos = ArMath::cos(-myTh);
  mySin = ArMath::sin(-myTh);
  myX = pose2.getX() - (myCos * pose1.getX() + mySin * pose1.getY());
  myY = pose2.getY() - (myCos * pose1.getY() - mySin * pose1.getX());
}

int ballDetection::moveToBall() {
	cout << "rufe Movetoball auf"<<endl;
	//cout << "KoordX: "<< KoordX<<endl;
	//int headingCount = 0;
	double obj = findObj();
	// kleiner wählen damit auf ball in größerer Entfernung gefunden wird
	while(obj >= 0.5) {
		p3dxptr->comInt(ArCommands::VEL, 0);
		//ballimBild = true;
		//cout << "#Drehungen: "<< headingCount << endl;

		ArPose currentPose = p3dxptr->getPose();
		cout<<"Koorigiere Heading "<<currentPose.getTh()<<endl;
		if(KoordX >= 0 && KoordX <= 399) {
			//p3dx.comInt(ArCommands::HEAD, i);
			cout<<"Setze Heading -10 "<<endl;
			//			headingCount++;
			p3dxptr->comInt(ArCommands::HEAD, (currentPose.getTh()+5));
			ArUtil::sleep(500);

		}

		if(KoordX > 399 && KoordX <= 799) {

			//p3dx.comInt(ArCommands::HEAD, i);
			cout<<"Setze Heading +10 "<<endl;
			//			headingCount++;
			p3dxptr->comInt(ArCommands::HEAD, currentPose.getTh()-5);
			ArUtil::sleep(500);


		}
		if(rotateToBall() == 1) {
			cout << "ball in der Mitte" <<endl;
			p3dxptr->comInt(ArCommands::VEL, 75);
			reached();
			cout << "Rückgabe Wert 1" << endl;
			return 1;
		}else {
			p3dxptr->comInt(ArCommands::VEL, 75);
			p3dxptr->comInt(ArCommands::RVEL, 0);
			cout << "Rückgabe Wert 0" << endl;
			return 0;
		}
	}
	if(obj < 0.5) {
		return 0;
	}	
}

/*-------------------------------------------------------------
					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
}

	virtual ArActionDesired *fire (ArActionDesired currentDesired)
	{
		if(BeenCorrectedByStar || !usestar) // if we have updated odo from star or is star isn't being used
		{
		 ArPose MyPose;
		 MyPose =	myRobot->getPose();
		Bottle PoseBottle;
		
		PoseBottle.addDouble(MyPose.getX()/1000);
		PoseBottle.addDouble(MyPose.getY()/1000);
		PoseBottle.addDouble(MyPose.getTh());
		Mycopyofmodule->SendBottleData("MAPout",PoseBottle);
		printf("my x:%f y:%f rot:%f \n",MyPose.getX()/1000,MyPose.getY()/1000,MyPose.getTh());
		}
	return &myDesired;
	}

/**
 * @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
}

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

ArPose fn1(void)
{
  static ArPose pose;
  pose.setX(pose.getX() + 1);
  pose.setY(pose.getY() + 1);
  pose.setTh(pose.getTh() - 1);
  return pose;
}

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

/*!
 * 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);
    }
  }
}

void ArSickLogger::internalPrintPos(ArPose poseTaken)
{
    if (myFile == NULL)
        return;
    ArPose encoderPose = myRobot->getEncoderPose();
    ArPose rawEncoderPose = myRobot->getRawEncoderPose();
    ArTransform normalToRaw(rawEncoderPose, encoderPose);

    ArPose rawPose;
    rawPose = normalToRaw.doInvTransform(poseTaken);
    fprintf(myFile, "#rawRobot: %.0f %.0f %.2f %.0f %.0f\n",
            rawPose.getX(),
            rawPose.getY(),
            rawPose.getTh(),
            myRobot->getVel(),
            myRobot->getRotVel());
    fprintf(myFile, "robot: %.0f %.0f %.2f %.0f %.0f\n",
            poseTaken.getX(),
            poseTaken.getY(),
            poseTaken.getTh(),
            myRobot->getVel(),
            myRobot->getRotVel());
}

/*-------------------------------------------------------------
					getOdometry
-------------------------------------------------------------*/
void CActivMediaRobotBase::getOdometry(poses::CPose2D &out_odom)
{
#if MRPT_HAS_ARIA
	ASSERTMSG_(THE_ROBOT!=NULL, "Robot is not connected")
	THE_ROBOT->lock();

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

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

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

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