コード例 #1
0
/// Slot called on completion of the Fit algorithm.
/// @param error :: Set to true if Fit finishes with an error.
void MultiDatasetFit::finishFit(bool error) {
  if (!error) {
    m_plotController->clear();
    m_plotController->update();
    Mantid::API::IFunction_sptr fun;
    auto algorithm = m_fitRunner->getAlgorithm();
    if (m_fitOptionsBrowser->getCurrentFittingType() ==
        MantidWidgets::FitOptionsBrowser::Simultaneous) {
      // After a simultaneous fit
      fun = algorithm->getProperty("Function");
      updateParameters(*fun);
      auto status =
          QString::fromStdString(algorithm->getPropertyValue("OutputStatus"));
      auto chiSquared = QString::fromStdString(
          algorithm->getPropertyValue("OutputChi2overDoF"));
      setFitStatusInfo(status, chiSquared);
      formatParametersForPlotting(
          *fun, algorithm->getPropertyValue("OutputParameters"));
    } else {
      // After a sequential fit
      auto paramsWSName =
          m_fitOptionsBrowser->getProperty("OutputWorkspace").toStdString();
      if (!Mantid::API::AnalysisDataService::Instance().doesExist(paramsWSName))
        return;
      size_t nSpectra = getNumberOfSpectra();
      if (nSpectra == 0)
        return;
      fun = m_functionBrowser->getGlobalFunction();
      auto nParams = fun->nParams() / nSpectra;
      auto params = Mantid::API::AnalysisDataService::Instance()
                        .retrieveWS<Mantid::API::ITableWorkspace>(paramsWSName);
      if (nParams * 2 + 2 != params->columnCount()) {
        throw std::logic_error(
            "Output table workspace has unexpected number of columns.");
      }
      for (size_t index = 0; index < nSpectra; ++index) {
        std::string prefix =
            "f" + boost::lexical_cast<std::string>(index) + ".";
        for (size_t ip = 0; ip < nParams; ++ip) {
          auto colIndex = ip * 2 + 1;
          auto column = params->getColumn(colIndex);
          fun->setParameter(prefix + column->name(), column->toDouble(index));
        }
      }
      updateParameters(*fun);
      showParameterPlot();
      clearFitStatusInfo();
    }
  }
  m_uiForm.btnFit->setEnabled(true);
}
コード例 #2
0
void EuphoriaWidget::keyPressEvent( QKeyEvent* e )
{
    if( e->key() == Qt::Key_0 ) { setDefaults( 0 ); updateParameters(); }
    if( e->key() == Qt::Key_1 ) { setDefaults( 1 ); updateParameters(); }
    if( e->key() == Qt::Key_2 ) { setDefaults( 2 ); updateParameters(); }
    if( e->key() == Qt::Key_3 ) { setDefaults( 3 ); updateParameters(); }
    if( e->key() == Qt::Key_4 ) { setDefaults( 4 ); updateParameters(); }
    if( e->key() == Qt::Key_5 ) { setDefaults( 5 ); updateParameters(); }
    if( e->key() == Qt::Key_6 ) { setDefaults( 6 ); updateParameters(); }
    if( e->key() == Qt::Key_7 ) { setDefaults( 7 ); updateParameters(); }
    if( e->key() == Qt::Key_8 ) { setDefaults( 8 ); updateParameters(); }
}
コード例 #3
0
void GaussianBlurFilter::onKeyPressed(int key) {
//    if (key==OF_KEY_LEFT) _blurSize--;
//    else if (key==OF_KEY_RIGHT) _blurSize++;
//    else if (key==OF_KEY_UP) _bloom += 0.1;
//    else if (key==OF_KEY_DOWN) _bloom -= 0.1;
   updateParameters();
}
コード例 #4
0
ファイル: KBootParms.C プロジェクト: BillTheBest/k42
KBootParms::KBootParms(const void *bootData, uval32 partRef)
    : parameterData(NULL), parameterDataLength(0), dataRef(partRef)
{
    if (!updateParameters(bootData, false)) {
	passertMsg(false, "Passed invalid data\n");
    }
}
コード例 #5
0
//==============================================================================
void DrumSynthEnvelope::resized ()
{
    xDelta = (getWidth ()) / MAX_ENVELOPE_LENGTH;
    yDelta = (getHeight ()) / MAX_ENVELOPE_GAIN;
    
    updateParameters (false);
コード例 #6
0
void UserSpecificQueryWrapperObject::setUserId(const QString &userId)
{
    if (m_userId != userId) {
        m_userId = userId;
        updateParameters();
        emit userIdChanged();
    }
}
コード例 #7
0
void BilateralFilter::onKeyPressed(int key) {
//    float blurOffset = _texelSpacing.x;
//    if (key==OF_KEY_DOWN) blurOffset -= 0.5f;
//    else if (key==OF_KEY_UP) blurOffset += 0.5f;
//    else if (key==OF_KEY_LEFT) _normalization -=0.5;
//    else if (key==OF_KEY_RIGHT) _normalization += 0.5;
    updateParameters();
}
コード例 #8
0
ファイル: EyeLinker.cpp プロジェクト: ofZach/funkyForms
// ------------- update
void EyeLinker::update(){
    updateVelocity();
    updateParameters();
    updatePhysics();
    updateEye();
    updateFading();
    updateFireworks();
}
コード例 #9
0
ptRecorder::ptRecorder() { // constructor for class JoyTeleop
  joySub = nh.subscribe("/joy", 10, &ptRecorder::joyCallback, this);
  poseSub = nh.subscribe("/robot_pose", 10, &ptRecorder::poseCallback, this);

  pointPub= nh.advertise<geometry_msgs::PoseStamped>("/pt", 10);

  updateParameters();
}
コード例 #10
0
ファイル: mb_estimator.c プロジェクト: RuinaLab/Ranger
/***************** ENTRY-POINT FUNCTION CALL  *****************************
 *                                                                        *
 **************************************************************************/
void mb_estimator_update(void) {
	clear_UI_LED();  // Clears all LEDs that had been active on the previous cycle

	if (INITIALIZE_ESTIMATOR) {
		// Initialize the filter coefficients:
		setFilterCoeff(&FC_FAST, FILTER_CUTOFF_FAST);
		setFilterCoeff(&FC_SLOW, FILTER_CUTOFF_SLOW);
		setFilterCoeff(&FC_VERY_SLOW, FILTER_CUTOFF_VERY_SLOW);

		// Reset the joint angle rate filters
		setFilterData(&FD_OUTER_LEG_ANGLE, ID_UI_ROLL);
		setFilterData(&FD_UI_ANG_RATE_X, ID_UI_ANG_RATE_X);
		setFilterData(&FD_MCH_ANG_RATE, ID_MCH_ANG_RATE);
		setFilterData(&FD_MCFO_RIGHT_ANKLE_RATE, ID_MCFO_RIGHT_ANKLE_RATE);
		setFilterData(&FD_MCFI_ANKLE_RATE, ID_MCFI_ANKLE_RATE);

		// Reset the contact sensor filters
		setFilterData(&FD_MCFO_LEFT_HEEL_SENSE, ID_MCFO_LEFT_HEEL_SENSE);
		setFilterData(&FD_MCFO_RIGHT_HEEL_SENSE, ID_MCFO_RIGHT_HEEL_SENSE);
		setFilterData(&FD_MCFI_LEFT_HEEL_SENSE, ID_MCFI_LEFT_HEEL_SENSE);
		setFilterData(&FD_MCFI_RIGHT_HEEL_SENSE, ID_MCFI_RIGHT_HEEL_SENSE);

		// Steering motor stuff:
		setFilterData(&FD_MCSI_STEER_ANGLE, ID_MCSI_STEER_ANGLE);

		// Robot orientation estimation
		resetRobotOrientation();
		getIntegralRateGyro();   // Run integral to log the current state of the rate gyro

		// Set "once per step" variables:
		STATE_lastStepLength = 0.0;    // Initialize to zero, for lack of a better plan
		STATE_lastStepTimeSec = STATE_t;  //  cpu clock time at last heel strike.
		STATE_lastStepDuration = 0.0;  // Duration of the last step (seconds)


		STATE_lastEstTime = 0.001 * mb_io_get_float(ID_TIMESTAMP);
		// Remember that we've initialized everything properly
		INITIALIZE_ESTIMATOR = false;
	}

	STATE_t = 0.001 * mb_io_get_float(ID_TIMESTAMP); // Robot Time (converted to seconds)
	runAllFilters();// Run the butterworth filters:
	updateRobotOrientation();
	sendTotalPower();
	updateEnergyUsage(); // Must come after sendTotalPower()

	// Update the state variables:  (absolute orientation and rate)
	updateRobotState();

	// Update controller parameters from LabVIEW
	updateParameters();

	// Check if the robot fell down
	checkIfRobotFellDown();

	STATE_lastEstTime = STATE_t; // Update previous estimation time.
	return;
}
コード例 #11
0
// executes serial implementation of stochastic gradient descent for
// logistic regression with a fixed number of iterations
// config_params: {step_size, characteristic_time}
void trainStochasticGradientDescent(
    DataSet training_set,
    TrainingOptions training_options) {

    // shuffle datapoints in order to add more stochasticity
    // shuffleKeyValue(training_set.data_points, training_set.labels,
    //                 training_set.num_data_points, training_set.num_features);

    FeatureType* gradient = new FeatureType[training_set.num_features];

    // read configuration parameters
    double step_size = *training_options.step_size;

    const double characteristic_time =
        (fieldExists(training_options.config_params, "characteristic_time"))
        ? training_options.config_params["characteristic_time"]
        : CHARACTERISTIC_TIME;

    size_t curr_num_epochs =
        (fieldExists(training_options.config_params, "curr_num_epochs"))
        ? training_options.config_params["curr_num_epochs"]
        : 0;

    double annealed_step_size = step_size;

    for (size_t k = 0; k < training_options.num_epochs; k++) {

        // simulated annealing (reduces step size as it converges)
        annealed_step_size = training_options.config_params["initial_step_size"]
                             / (1.0
                                + (curr_num_epochs
                                   * training_set.num_data_points
                                   / characteristic_time));
        curr_num_epochs++;

        for (size_t i = 0; i < training_set.num_data_points; i++) {
            // computes gradient
            gradientForSinglePoint(
                training_set.parameter_vector,
                &training_set.data_points[i * training_set.num_features],
                training_set.labels[i],
                training_set.num_features,
                gradient);

            // updates parameter vector
            updateParameters(
                training_set.parameter_vector,
                gradient,
                training_set.num_features,
                annealed_step_size);
        }
    }

    *training_options.step_size = annealed_step_size;

    delete[] gradient;
}
コード例 #12
0
// Bold Driver: adjusting the step size according to the result of the
// last step and reverting the step if results are worse than they were before.
static void boldDriver(
    DataSet training_set,
    FeatureType* gradient,
    double* step_size) {

    double previous_loss = lossFunction(training_set);

    updateParameters(training_set.parameter_vector,
                     gradient,
                     training_set.num_features,
                     *step_size);

    double current_loss = lossFunction(training_set);

    // if it's going in the right direction, increase step size
    if (current_loss < previous_loss) {
        *step_size *= 1.045;
    }
    // if the previous step was too big and the loss increased,
    // revert step and reduce step size
    else {
        bool revert = true;
        int num_reverts = 0, max_reverts = 10;
        while (revert && (num_reverts < max_reverts)) {
            updateParameters(training_set.parameter_vector,
                             gradient,
                             training_set.num_features,
                             *step_size,
                             revert);

            *step_size *= 0.5;

            updateParameters(training_set.parameter_vector,
                             gradient,
                             training_set.num_features,
                             *step_size);

             current_loss = lossFunction(training_set);

             revert = (current_loss > previous_loss);
        }

    }
}
コード例 #13
0
ファイル: ltiFunctor.cpp プロジェクト: chenbk85/alcordev
  bool functor::useParameters(functor::parameters& theParams) {
    if (ownParams) {
      delete params;
      params = 0;
    }
    params = &theParams;
    ownParams = false;

    return updateParameters();
  }
コード例 #14
0
// Window initialization
void EuphoriaWidget::initializeGL()
{
    // Need to call this to setup viewport[] parameters used in
    // the next updateParameters() call
    resizeGL( width(), height() );

    updateParameters();

    _timer->start( _frameTime, true );
}
コード例 #15
0
ファイル: Unison.cpp プロジェクト: xsgoodbox/zynaddsubfx
void Unison::setSize(int new_size)
{
    if(new_size < 1)
        new_size = 1;
    unison_size = new_size;
    alloc.devalloc(uv);
    uv = alloc.valloc<UnisonVoice>(unison_size);
    first_time = true;
    updateParameters();
}
コード例 #16
0
void PanelObjectEvent::on_radioButtonEventUser_toggled(bool checked) {
    ui->comboBoxEventsUser->setEnabled(checked);
    m_event->setIsSystem(false);
    QStandardItemModel *model = RPM::get()->project()->gameDatas()
        ->commonEventsDatas()->modelEventsUser();
    SystemEvent *super = reinterpret_cast<SystemEvent *>(model->item(ui
        ->comboBoxEventsUser->currentIndex())->data().value<quintptr>());
    updateEvent(super);
    updateParameters(super);
}
コード例 #17
0
void PanelObjectEvent::on_comboBoxEventsUserCurrentIndexChanged(int index) {
    QStandardItemModel *model = RPM::get()->project()->gameDatas()
        ->commonEventsDatas()->modelEventsUser();
    SystemEvent *super = reinterpret_cast<SystemEvent *>(model->item(index)
        ->data().value<quintptr>());
    if (!m_event->isSystem()) {
        updateEvent(super);
        updateParameters(super);
    }
}
コード例 #18
0
ファイル: ltiFunctor.cpp プロジェクト: chenbk85/alcordev
  /* sets the functor's parameters.
     The functor keeps its own copy of the given parameters.*/
  bool functor::setParameters(const functor::parameters& theParams) {
    if (ownParams) {
      delete params;
      params = 0;
    }
    params = theParams.clone();
    ownParams = true;

    return updateParameters();
  }
コード例 #19
0
void ColorGradationPlugin::changedParam( const OFX::InstanceChangedArgs& args, const std::string& paramName )
{
	if( paramName == kParamInvert )
	{
		int in = _paramIn->getValue();
		_paramIn->setValue ( _paramOut->getValue() );
		_paramOut->setValue( in );
	}
	updateParameters();
}
コード例 #20
0
ファイル: Unison.cpp プロジェクト: CallisteHanriat/lmms
void Unison::setSize(int new_size)
{
    if(new_size < 1)
        new_size = 1;
    unison_size = new_size;
    if(uv)
        delete [] uv;
    uv = new UnisonVoice[unison_size];
    first_time = true;
    updateParameters();
}
コード例 #21
0
ファイル: MovingDots.cpp プロジェクト: benvermaercke/mworks
void MovingDots::drawFrame(shared_ptr<StimulusDisplay> display) {
    //
    // If we're drawing to the main display, update dots
    //
    
    if (display->getCurrentContextIndex() == 0) {
        currentTime = getElapsedTime();
        if (previousTime != currentTime) {
            updateParameters();
            updateDots();
            previousTime = currentTime;
        }
    }
    
    if (0 == currentNumDots) {
        // No dots, so nothing to draw
        return;
    }
    
    //
    // Draw the dots
    //
    
    glPushMatrix();
    glTranslatef(fieldCenterX->getValue().getFloat(), fieldCenterY->getValue().getFloat(), 0.0f);
    glScalef(currentFieldRadius, currentFieldRadius, 1.0f);
    glRotatef(direction->getValue().getFloat(), 0.0f, 0.0f, 1.0f);
    
    // Enable antialiasing so dots are round, not square
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    glBlendEquation(GL_FUNC_ADD);
    glEnable(GL_POINT_SMOOTH);
    glHint(GL_POINT_SMOOTH_HINT, GL_FASTEST);
    
    glColor4f(red->getValue().getFloat(),
              green->getValue().getFloat(),
              blue->getValue().getFloat(),
              alpha->getValue().getFloat());

    glEnableClientState(GL_VERTEX_ARRAY);

    glPointSize(dotSize->getValue().getFloat() * dotSizeToPixels[display->getCurrentContextIndex()]);
    glVertexPointer(verticesPerDot, GL_FLOAT, 0, &(dotPositions[0]));
    glDrawArrays(GL_POINTS, 0, currentNumDots);

    glDisableClientState(GL_VERTEX_ARRAY);
    
    glDisable(GL_BLEND);
    glDisable(GL_POINT_SMOOTH);
    
    glPopMatrix();
}
コード例 #22
0
ファイル: Unison.cpp プロジェクト: CallisteHanriat/lmms
void Unison::setBandwidth(float bandwidth)
{
    if(bandwidth < 0)
        bandwidth = 0.0f;
    if(bandwidth > 1200.0f)
        bandwidth = 1200.0f;

    /* If the bandwidth is too small, the audio may cancel itself out
     * (due to the sign change of the outputs)
     * TODO figure out the acceptable lower bound and codify it
     */
    unison_bandwidth_cents = bandwidth;
    updateParameters();
}
コード例 #23
0
bool EngineEffectChain::processEffectsRequest(const EffectsRequest& message,
                                              EffectsResponsePipe* pResponsePipe) {
    EffectsResponse response(message);
    switch (message.type) {
        case EffectsRequest::ADD_EFFECT_TO_CHAIN:
            if (kEffectDebugOutput) {
                qDebug() << debugString() << "ADD_EFFECT_TO_CHAIN"
                         << message.AddEffectToChain.pEffect
                         << message.AddEffectToChain.iIndex;
            }
            response.success = addEffect(message.AddEffectToChain.pEffect,
                                         message.AddEffectToChain.iIndex);
            break;
        case EffectsRequest::REMOVE_EFFECT_FROM_CHAIN:
            if (kEffectDebugOutput) {
                qDebug() << debugString() << "REMOVE_EFFECT_FROM_CHAIN"
                         << message.RemoveEffectFromChain.pEffect
                         << message.RemoveEffectFromChain.iIndex;
            }
            response.success = removeEffect(message.RemoveEffectFromChain.pEffect,
                                            message.RemoveEffectFromChain.iIndex);
            break;
        case EffectsRequest::SET_EFFECT_CHAIN_PARAMETERS:
            if (kEffectDebugOutput) {
                qDebug() << debugString() << "SET_EFFECT_CHAIN_PARAMETERS"
                         << "enabled" << message.SetEffectChainParameters.enabled
                         << "mix" << message.SetEffectChainParameters.mix;
            }
            response.success = updateParameters(message);
            break;
        case EffectsRequest::ENABLE_EFFECT_CHAIN_FOR_CHANNEL:
            if (kEffectDebugOutput) {
                qDebug() << debugString() << "ENABLE_EFFECT_CHAIN_FOR_CHANNEL"
                         << message.channel;
            }
            response.success = enableForChannel(message.channel);
            break;
        case EffectsRequest::DISABLE_EFFECT_CHAIN_FOR_CHANNEL:
            if (kEffectDebugOutput) {
                qDebug() << debugString() << "DISABLE_EFFECT_CHAIN_FOR_CHANNEL"
                         << message.channel;
            }
            response.success = disableForChannel(message.channel);
            break;
        default:
            return false;
    }
    pResponsePipe->writeMessages(&response, 1);
    return true;
}
コード例 #24
0
ファイル: addaction.cpp プロジェクト: serghei/kde3-kdeutils
void AddAction::updateProfileFunctions()
{
	ProfileServer *theServer = ProfileServer::profileServer();
	theProfileFunctions->clear();
	profileFunctionMap.clear();
	if(!theProfiles->currentItem()) return;

	const Profile *p = theServer->profiles()[profileMap[theProfiles->currentItem()]];
	QDict<ProfileAction> dict = p->actions();
	for(QDictIterator<ProfileAction> i(dict); i.current(); ++i)
		profileFunctionMap[new QListViewItem(theProfileFunctions, i.current()->name(), QString().setNum(i.current()->arguments().count()), i.current()->comment())] = i.currentKey();
	updateParameters();
	updateOptions();
}
コード例 #25
0
ファイル: DialogLoadMBLUE.cpp プロジェクト: prohan91/zyGrib
//-------------------------------------------------------------------------------
void DialogLoadMBLUE::slotParameterUpdated()
{
    updateParameters();

	int npts = (int) (ceil(fabs(xmax-xmin))*ceil(fabs(ymax-ymin))*400 );
    
    int nbrec = (int) days*24/interval +1;
	
	double nbdata = 0;
	
	nbdata += wind  ?    4.4 / 3.6 : 0;
	nbdata += GUSTsfc ?  3.4 / 5.7 : 0;
	nbdata += pressure ? 3.4 / 4.2 : 0;
	nbdata += rain  ?    3.4 / 8.0 : 0;
	nbdata += cloud ?    3.4 / 9.9 : 0;
	nbdata += temp  ?    3.4 / 3.9 : 0;
	nbdata += humid ?    3.4 / 5.3 : 0;
	nbdata += CAPEsfc ?  3.4 / 7.9 : 0;

	if (chkAltitude500->isChecked()) nbdata += 6.3 / 3.3;
	if (chkAltitude700->isChecked()) nbdata += 6.3 / 3.3;
	if (chkAltitude850->isChecked()) nbdata += 6.3 / 3.3; 
    
    int head = (nbdata>0) ? 512 : 0;
	
    double estime = head + nbdata*4*nbrec*npts; 
	
    // compress
/*    if (estime < 2000) estime /= 16;
    else if (estime < 6000) estime /= 14;
    else if (estime < 15000) estime /= 8;
    else if (estime < 100000) estime /= 6; 
    else estime /= 5;*/

//    estime /= 3;    // zip
	
    estime = estime/1024;	// ko
// DBG ("estime= %d ko", (int)estime);

	double compress;
	compress = 4.2;
	slotGribMessage(tr("Size: %1 ko approx").arg((int)(estime/compress)) );
    
    if (estime == 0)
        btOK->setEnabled(false);
    else
        btOK->setEnabled(true);
	
}
コード例 #26
0
 void msPhysicalInterface::setParameter(string key,string value){
     
     LOGGER_ENTER_FUNCTION_DBG("void msPhysicalInterface::setParameter(string key,string value)", getFullId());
     
     try{
         Parameters->setParameter(key,value,*Units);
         updateParameters();
     }
     catch(msException& e){
         
         e.addContext("void msPhysicalInterface::setParameter(string key,string value)");
     }
     LOGGER_EXIT_FUNCTION2("void msPhysicalInterface::setParameter(string key,string value)");
     
 }
コード例 #27
0
void DRowAudioFilter::setScaledParameter (int index, float newValue)
{
	for (int i = 0; i < noParams; i++)
	{
		if (index == i) {
			if (params[i].getValue() != newValue) {
				params[i].setValue(newValue);
				sendChangeMessage (this);
			}
		}
	}
	
	updateParameters();
	updateFilters();
}
コード例 #28
0
ファイル: osgsimulation.cpp プロジェクト: yueying/osg
    virtual void operator()(osg::Node* node, osg::NodeVisitor* nv)
    {
        updateParameters();

        osg::NodePath nodePath = nv->getNodePath();

        osg::MatrixTransform* mt = nodePath.empty() ? 0 : dynamic_cast<osg::MatrixTransform*>(nodePath.back());
        if (mt)
        {
            osg::CoordinateSystemNode* csn = 0;

            // find coordinate system node from our parental chain
            unsigned int i;
            for(i=0; i<nodePath.size() && csn==0; ++i)
            {
                csn = dynamic_cast<osg::CoordinateSystemNode*>(nodePath[i]);
            }

            if (csn)
            {


                osg::EllipsoidModel* ellipsoid = csn->getEllipsoidModel();
                if (ellipsoid)
                {
                    osg::Matrix inheritedMatrix;
                    for(i+=1; i<nodePath.size()-1; ++i)
                    {
                        osg::Transform* transform = nodePath[i]->asTransform();
                        if (transform) transform->computeLocalToWorldMatrix(inheritedMatrix, nv);
                    }

                    osg::Matrixd matrix(inheritedMatrix);

                    //osg::Matrixd matrix;
                    ellipsoid->computeLocalToWorldTransformFromLatLongHeight(_latitude,_longitude,_height,matrix);
                    matrix.preMultRotate(_rotation);

                    mt->setMatrix(matrix);
                }

            }
        }

        traverse(node,nv);
    }
コード例 #29
0
    void processOdometryNoGps(const nav_msgs::Odometry::ConstPtr& msg){
        if(close_to_goal==0){
            prev_x = msg->pose.pose.position.x;
            prev_y = msg->pose.pose.position.y;
            qx=msg->pose.pose.orientation.x;
            qy=msg->pose.pose.orientation.y;
            qz=msg->pose.pose.orientation.z;
            qw=msg->pose.pose.orientation.w;
            prev_yaw = atan2(2*(qx*qy + qw*qz),qw*qw + qx*qx -qy*qy - qz*qz);
        }
        else{
            current_x = msg->pose.pose.position.x;
            current_y = msg->pose.pose.position.y;
            qx=msg->pose.pose.orientation.x;
            qy=msg->pose.pose.orientation.y;
            qz=msg->pose.pose.orientation.z;
            qw=msg->pose.pose.orientation.w;
            current_yaw = atan2(2*(qx*qy + qw*qz),qw*qw + qx*qx -qy*qy - qz*qz);

            d_xm = current_x - prev_x;
            d_ym = current_y - prev_y;
            d_yaw = current_yaw - prev_yaw;
            yaw   = yaw + d_yaw;
            correct_angle(yaw);
            d_x = sqrt(d_xm*d_xm +d_ym*d_ym)*cos(yaw);
            d_y = sqrt(d_xm*d_xm +d_ym*d_ym)*sin(yaw);
            prev_x = current_x;
            prev_y = current_y;
            prev_yaw = current_yaw;
            active_pose.pose.pose.position.x= pos_x = pos_x + d_x;
            active_pose.pose.pose.position.y= pos_y = pos_y + d_y;


            active_pose.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
            active_pose_pub.publish(active_pose);
            if(hasGoal){
                updateParameters();
                publishSystemInput();
            }
            else{
                publishGoalReached();
            }

        }

	}
コード例 #30
0
ファイル: jaccount.cpp プロジェクト: rnz/qutim
void JAccount::virtual_hook(int id, void *data)
{
	switch (id) {
	case ReadParametersHook: {
		QVariantMap &parameters = *reinterpret_cast<QVariantMap*>(data);
		parameters = d_func()->parameters;
		break;
	}
	case UpdateParametersHook: {
		UpdateParametersArgument &argument = *reinterpret_cast<UpdateParametersArgument*>(data);
		argument.reconnectionRequired = updateParameters(argument.parameters);
		break;
	}
	default:
		Account::virtual_hook(id, data);
	}
}