C++ StatePtr类代码示例

void StateManager::print_trace_aux(StatePtr p)	// changes by Uli
{
  state original;
  char *s;

  if (p.isStart()) {
    // this is a startstate
    // expand it into global variable `theworld`
    // StateCopy(workingstate, s);   // Uli: workingstate is set in 
    //      StateName()

    // output startstate
    cout << "Startstate " << (s = StartState->StateName(p))
	<< " fired.\n";
    delete[]s;			// Uli: avoid memory leak
    theworld.print();
    cout << "----------\n\n";
  } else {
    // print the prefix
    print_trace_aux(p.previous());

    // print the next state, which should be equivalent to state s
    // and set theworld to that state.
    // FALSE: no need to print full state
    Rules->print_world_to_state(p, FALSE);
  }
}

  bool FootstepGraph::isGoal(StatePtr state)
  {
    FootstepState::Ptr goal = getGoal(state->getLeg());
    if (publish_progress_) {
      jsk_footstep_msgs::FootstepArray msg;
      msg.header.frame_id = "odom"; // TODO fixed frame_id
      msg.header.stamp = ros::Time::now();
      msg.footsteps.push_back(*state->toROSMsg());
      pub_progress_.publish(msg);
    }
    Eigen::Affine3f pose = state->getPose();
    Eigen::Affine3f goal_pose = goal->getPose();
    Eigen::Affine3f transformation = pose.inverse() * goal_pose;

    if ((parameters_.goal_pos_thr > transformation.translation().norm()) &&
        (parameters_.goal_rot_thr > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()))) {
      // check collision
      if (state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
        if (right_goal_state_->crossCheck(state)) {
          return true;
        }
      } else if (state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
        if (left_goal_state_->crossCheck(state)) {
          return true;
        }
      }
    }
    return false;
  }

  bool FootstepGraph::finalizeSteps(const StatePtr &last_1_Step, const StatePtr &lastStep,
                                    std::vector<StatePtr> &finalizeSteps) {
    // simple finalize (no check)
    if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
      finalizeSteps.push_back(right_goal_state_);
      finalizeSteps.push_back(left_goal_state_);
    } else if (lastStep->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
      finalizeSteps.push_back(left_goal_state_);
      finalizeSteps.push_back(right_goal_state_);
    }

    return true;
  }

void StateManager::print_trace(StatePtr p)
{
  // print the prefix 
  if (p.isStart()) {
    print_trace_aux(p);
  } else {
    print_trace_aux(p.previous());

    // print the next state, which should be equivalent to state s
    // and set theworld to that state.
    // TRUE: print full state please;
    Rules->print_world_to_state(p, TRUE);
  }
}

  bool FootstepGraph::successors_original(StatePtr target_state, std::vector<FootstepGraph::StatePtr> &ret)
  {
    std::vector<Eigen::Affine3f> transformations;
    int next_leg;
    if (target_state->getLeg() == jsk_footstep_msgs::Footstep::LEFT) {
      transformations = successors_from_left_to_right_;
      next_leg = jsk_footstep_msgs::Footstep::RIGHT;
    }
    else if (target_state->getLeg() == jsk_footstep_msgs::Footstep::RIGHT) {
      transformations = successors_from_right_to_left_;
      next_leg = jsk_footstep_msgs::Footstep::LEFT;
    }
    else {
      // TODO: error
    }

    //std::vector<FootstepGraph::StatePtr> ret;
    Eigen::Affine3f base_pose = target_state->getPose();
    for (size_t i = 0; i < transformations.size(); i++) {
      Eigen::Affine3f transform = transformations[i];
      FootstepGraph::StatePtr next(new FootstepState(next_leg,
                                                     base_pose * transform,
                                                     target_state->getDimensions(),
                                                     resolution_));
      if (use_pointcloud_model_ && !lazy_projection_) {
        // Update footstep position by projection
        unsigned int error_state;
        FootstepGraph::StatePtr tmpnext = projectFootstep(next, error_state);
        if (!tmpnext && localMovement() && error_state == projection_state::close_to_success) {
          std::vector<StatePtr> locally_moved_nodes = localMoveFootstepState(next);
          for (size_t j = 0; j < locally_moved_nodes.size(); j++) {
            if (isSuccessable(locally_moved_nodes[j], target_state)) {
              FootstepGraph::StatePtr tmp = projectFootstep(locally_moved_nodes[j], error_state);
              if(!!tmp) {
                ret.push_back(tmp);
              }
            }
          }
        }
        next = tmpnext;
      }
      if (!!next) {
        if (isSuccessable(next, target_state)) {
          ret.push_back(next);
        }
      }
    }
    return true;
  }

void StateManager::push(StatePtr state)
{
    state->initialize();

    // NOTE: should this check for duplicate states?
    _stateStack.push(state);
}

 bool FootstepGraph::isGoal(StatePtr state)
 {
   FootstepState::Ptr goal = getGoal(state->getLeg());
   if (publish_progress_) {
     jsk_footstep_msgs::FootstepArray msg;
     msg.header.frame_id = "odom";
     msg.header.stamp = ros::Time::now();
     msg.footsteps.push_back(*state->toROSMsg());
     pub_progress_.publish(msg);
   }
   Eigen::Affine3f pose = state->getPose();
   Eigen::Affine3f goal_pose = goal->getPose();
   Eigen::Affine3f transformation = pose.inverse() * goal_pose;
   return (pos_goal_thr_ > transformation.translation().norm()) &&
     (rot_goal_thr_ > std::abs(Eigen::AngleAxisf(transformation.rotation()).angle()));
 }

void Mind::PushState( const StatePtr &state ) {
	assert( state != NULL );
	// Push the state to the front of the queue
	_stateQueue.push_front( state );
	state->SetOwner( _owner.GetEntity() );
	// Trigger a stateswitch next round
	_switchState = true;
	//PrintStateQueue("push");
}

 void GameController::setState(const StatePtr & statePtr) {
     if(statePtr) {
         state->onExit();
         HIKARI_LOG(debug) << "<GameController> exited \"" << state->getName() << "\" state.";
         prevState = state->getName();
         currState = statePtr->getName();
         state = statePtr;
         state->onEnter();
         HIKARI_LOG(debug) << "<GameController> entered \"" << state->getName() << "\" state.";
     }
 }

StatePtr SimpleTexturedMaterial::makeState(void)
{
    StatePtr state = Inherited::makeState();

    prepareLocalChunks();

    state->addChunk(_textureChunk);
    state->addChunk(_texGenChunk);

    if(getImage() != NullFC &&
       getImage()->hasAlphaChannel() && 
       getEnvMode() != GL_DECAL)
    {
        if(getImage()->isAlphaBinary())
        {
            if(_blendChunk->getSrcFactor() == GL_SRC_ALPHA)
            {
                beginEditCP(_blendChunk);
                _blendChunk->setSrcFactor(GL_ONE);
                _blendChunk->setDestFactor(GL_ZERO);
                _blendChunk->setAlphaFunc(GL_NOTEQUAL);
                _blendChunk->setAlphaValue(0);          
                endEditCP(_blendChunk);            
            }
        }
        else
        {
            if(_blendChunk->getSrcFactor() != GL_SRC_ALPHA)
            {
                beginEditCP(_blendChunk);
                _blendChunk->setSrcFactor(GL_SRC_ALPHA);
                _blendChunk->setDestFactor(GL_ONE_MINUS_SRC_ALPHA);
                _blendChunk->setAlphaFunc(GL_NONE);
                _blendChunk->setAlphaValue(0);          
                endEditCP(_blendChunk);                        
            }
        }
    }

    return state;
}

// Activate callback
void TestState::activate(cycle_t cyc)
{
	// Display callback text
	DISPLAY(cout << "TestState: In activate()" << endl);

	// Test trying to call callback-protected functions (this is a callback...)
	StatePtr snew = NewStateInstance<KickBallState>(sc, 0);
	EXPECT_EQ(SCR_BAD_CALLBACK, sc->init(snew));
	EXPECT_EQ(SCR_BAD_CALLBACK, sc->forceState(snew));
	EXPECT_EQ(SCR_BAD_CALLBACK, sc->step());
	EXPECT_EQ(SCR_BAD_CALLBACK, sc->loop());
	snew.reset();
	
	// Refuse to activate this state by calling terminate() (...at least the first time this function is called)
	if(!sc->g_tested_term)
	{
		DISPLAY(cout << "TestState: Calling terminate() from inside activate()" << endl);
		sc->g_tested_term = true;
		sc->terminate();
		return; // Just here for good coding standard after a terminate()
	}
}

IECore::RunTimeTypedPtr ToGLStateConverter::doConversion( IECore::ConstObjectPtr src, IECore::ConstCompoundObjectPtr operands ) const
{
	const CompoundObject *co = runTimeCast<const CompoundObject>( src.get() );
	if( !co )
	{
		throw Exception( "Expected a CompoundObject" );
	}

	const AttributeToStateMap &m = attributeToStateMap();

	const StatePtr result = new State( false );
	for( CompoundObject::ObjectMap::const_iterator it = co->members().begin(), eIt = co->members().end(); it != eIt; ++it )
	{
		AttributeToStateMap::const_iterator mIt = m.find( it->first );
		if( mIt != m.end() )
		{
			StateComponentPtr s = mIt->second( it->second.get() );
			result->add( s );
		}
	}
	return result;
}

StatePtr Game::popBackState() {
    StatePtr popped = mvStates.back();
    popped->dispose();
    mvStates.pop_back();
    return popped;
}

/********************
 Auxiliary function for error trace printing
 ********************/
bool match(state* ns, StatePtr p)
{
  int i;
  static PermSet Perm;
  static state temp;
  StateCopy(&temp, ns);
  if (args->symmetry_reduction.value)
    {
      if (  args->sym_alg.mode == argsym_alg::Exhaustive_Fast_Canonicalize) {
        Perm.ResetToExplicit();
        for (i=0; i<Perm.count; i++)
          if (Perm.In(i))
            {
              if (ns != workingstate)
                  StateCopy(workingstate, ns);
              
              mu_v.Permute(Perm,i);
              if (args->multiset_reduction.value)
                mu_v.MultisetSort();
            if (p.compare(workingstate)) {
              StateCopy(workingstate,&temp); return TRUE; }
          }
        StateCopy(workingstate,&temp);
        return FALSE;
      }
      else {
        Perm.ResetToSimple();
        Perm.SimpleToOne();
        if (ns != workingstate)
          StateCopy(workingstate, ns);

          mu_v.Permute(Perm,0);
          if (args->multiset_reduction.value)
            mu_v.MultisetSort();
        if (p.compare(workingstate)) {
          StateCopy(workingstate,&temp); return TRUE; }

        while (Perm.NextPermutation())
          {
            if (ns != workingstate)
              StateCopy(workingstate, ns);
              
              mu_v.Permute(Perm,0);
              if (args->multiset_reduction.value)
                mu_v.MultisetSort();
            if (p.compare(workingstate)) {
              StateCopy(workingstate,&temp); return TRUE; }
          }
        StateCopy(workingstate,&temp);
        return FALSE;
      }
    }
  if (!args->symmetry_reduction.value
      && args->multiset_reduction.value)
    {
      if (ns != workingstate)
          StateCopy(workingstate, ns);
      mu_v.MultisetSort();
      if (p.compare(workingstate)) {
        StateCopy(workingstate,&temp); return TRUE; }
      StateCopy(workingstate,&temp);
      return FALSE;
    }
  return (p.compare(ns));
}

 Eigen::Affine3f FootstepGraph::getRobotCoords(StatePtr current_state, StatePtr previous_state) const
 {
   Eigen::Affine3f mid = current_state->midcoords(*previous_state);
   return mid * collision_bbox_offset_;
 }