void EditionToolView::react_edition_end() { deactivate(); }
void ComeHitherNavigationTool::buttonCallback(int,InputDevice::ButtonCallbackData* cbData) { if(cbData->newButtonState) // Button has just been pressed { if(isActive()) // Tool is already active { /* Snap to the target transformation immediately: */ setNavigationTransformation(targetNav); /* Deactivate this tool: */ deactivate(); } else // Tool is not yet active { /* Try activating this tool: */ if(activate()) { /* Get the start navigation transformation: */ startNav=getNavigationTransformation(); startTime=getApplicationTime(); /* Get the target transformation: */ NavTransform device=getButtonDeviceTransformation(0); device.leftMultiply(getInverseNavigationTransformation()); Point center=device.getOrigin(); Vector forward=device.getDirection(1); Vector up=device.getDirection(2); /* Compute the navigation transformation for the target transformation: */ targetNav=NavTransform::identity; targetNav*=NavTransform::translateFromOriginTo(getDisplayCenter()); targetNav*=NavTransform::rotate(Rotation::fromBaseVectors(Geometry::cross(getForwardDirection(),getUpDirection()),getForwardDirection())); targetNav*=NavTransform::scale(startNav.getScaling()); targetNav*=NavTransform::rotate(Geometry::invert(Rotation::fromBaseVectors(Geometry::cross(forward,up),forward))); targetNav*=NavTransform::translateToOriginFrom(center); /* Compute the linear and angular velocities for the movement: */ NavTransform delta=startNav; delta.doInvert(); delta.leftMultiply(targetNav); Vector linearDist=delta.getTranslation(); double linearMag=Geometry::mag(linearDist); Vector angularDist=delta.getRotation().getScaledAxis(); double angularMag=Geometry::mag(angularDist); if(linearMag<=factory->linearSnapThreshold&&angularMag<=factory->angularSnapThreshold) { /* Snap to the target transformation immediately: */ setNavigationTransformation(targetNav); /* Deactivate this tool: */ deactivate(); } else { /* Compute the total transition time: */ double transitionTime=linearMag/factory->maxLinearVelocity; if(transitionTime<angularMag/factory->maxAngularVelocity) transitionTime=angularMag/factory->maxAngularVelocity; endTime=startTime+transitionTime; /* Compute the effective linear and angular velocities: */ linearVelocity=linearDist/Scalar(transitionTime); angularVelocity=angularDist/Scalar(transitionTime); } } } } }
void CCSceneObject::deleteLater() { // Mark to delete in 2 frames deleteMe = 2; deactivate(); }
void DL_wheel::new_frame(void) { DL_vector vtmp; wx.times(wr*cos(wa),&vtmp); wc.plus(&vtmp,&wpc); wy.times(wr*sin(wa),&vtmp); wpc.plus(&vtmp,&wpc); st_inbounds=surf->pos(s,t,&spc); #ifdef WA2 DL_Scalar wa2,s2,t2,ast=DL_dsystem->get_integrator()->ast(); wa2=wa+ast*(wa-oldwa); s2=s+ast*(s-olds); t2=t+ast*(t-oldt); wx.times(wr*cos(wa2),&vtmp); wc.plus(&vtmp,&wpc2); wy.times(wr*sin(wa2),&vtmp); wpc2.plus(&vtmp,&wpc2); st_inbounds=surf->pos(s2,t2,&spc2); #endif if (surf->get_geo()) { DL_vector snl; surf->deriv0(s,t,&sx); surf->deriv1(s,t,&sy); sx.crossprod(&sy,&snl); surf->get_geo()->to_world(&snl,&snw); surf->get_geo()->to_world(&sx,&sxw); surf->get_geo()->to_world(&sy,&syw); } else { surf->deriv0(s,t,&sxw); surf->deriv1(s,t,&syw); sxw.crossprod(&syw,&snw); } snw.normalize(); // calculate rrs en rrt DL_vector rr; { DL_vector dwp; wx.times(-wr*sin(wa),&dwp); wy.times(wr*cos(wa),&vtmp); dwp.plusis(&vtmp); w->to_world(&dwp,&rr); } snw.times(-rr.inprod(&snw),&vtmp); rr.plusis(&vtmp); rr.normalize(); w->to_world(&wn,&vtmp); DL_Scalar ftmp=vtmp.inprod(&snw); // if the angle between the wheel's normal and the surface normal // is very small (ftmp close to 1), then the wheel is almost flat // on the surface. This constraint type is not made for such situation, // so then deactivate this constraint: if (ftmp>0.97) {// angle smaller than ~14 degrees DL_dsystem->get_companion()->Msg("Wheel in wheel-constraint too flat: deactivating constraint\n"); deactivate(); return; } //#define THICKWHEEL #ifdef THICKWHEEL // apply correction for sideways elipsoid to wpc: DL_vector a; DL_Scalar beta=-ftmp; // limit the elipsoid angle to a ten degree maximum: #define MAXANG 0.1 if (beta<-MAXANG) beta=-MAXANG; if (beta>MAXANG) beta=MAXANG; #undef MAXANG #ifdef WA2 wpc2.minus(&wc,&a); a.normalize(); wn.times(wr*sin(beta),&vtmp); wpc2.plus(&vtmp,&wpc); a.times(wr*(cos(beta)-1),&vtmp); wpc2.plus(&vtmp,&wpc); #else wpc.minus(&wc,&a); a.normalize(); wn.times(wr*sin(beta),&vtmp); wpc.plus(&vtmp,&wpc); a.times(wr*(cos(beta)-1),&vtmp); wpc.plus(&vtmp,&wpc); #endif #endif { DL_point wcw,spcw; w->to_world(&wc,&wcw); if (surf->get_geo()) surf->get_geo()->to_world(&spc,&spcw); else spcw.assign(&spc); wcw.minus(&spcw,&vtmp); } vtmp.normalize(); DL_vector angv; angv.assign(w->get_angvelocity()); angv.normalize(); #ifdef CIRCLEPATH straight=(fabs(angv.inprod(&vtmp))<0.1); if (straight) { #endif rrs=wr*rr.inprod(&sxw)/sxw.inprod(&sxw); rrt=wr*rr.inprod(&syw)/syw.inprod(&syw); #ifdef CIRCLEPATH } else { DL_Scalar rs; DL_vector vtmp2; rr.crossprod(&snw,&vtmp2); vtmp2.normalize(); rs=fabs(wr*angv.inprod(&vtmp2)/angv.inprod(&vtmp)); wrsr=wr/rs; sa=acos(rr.inprod(&syw)/syw.norm()); if (rr.inprod(&sxw)>0) sa=-sa; rrs=rs/sxw.norm(); rrt=rs/syw.norm(); if ((ftmp<0)==up) { sa+=3.141592654; wrsr=-wrsr; } } #endif DL_Scalar delta=wa-oldwa; oldwa=wa; wa+=delta; olds=s; oldt=t; #ifdef CIRCLEPATH if (straight) { #endif s+=rrs*delta; t+=rrt*delta; #ifdef CIRCLEPATH } else { DL_Scalar saold=sa; sa+=delta*wrsr; s+=rrs*(cos(sa)-cos(saold)); t+=rrt*(sin(sa)-sin(saold)); } #endif DL_constraint::new_frame(); }
void MouseNavigationTool::valuatorCallback(int,InputDevice::ValuatorCallbackData* cbData) { currentValue=Scalar(cbData->newValuatorValue); if(currentValue!=Scalar(0)) { /* Act depending on this tool's current state: */ switch(navigationMode) { case IDLE: case SPINNING: /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) { if(dolly) { /* Calculate the dollying direction: */ if(mouseAdapter!=0) dollyDirection=mouseAdapter->getWindow()->getViewer()->getHeadPosition()-calcScreenCenter(); else dollyDirection=getMainViewer()->getHeadPosition()-calcScreenCenter(); dollyDirection.normalize(); /* Initialize the wheel dollying factor: */ currentWheelScale=Scalar(1); preScale=getNavigationTransformation(); /* Go to wheel dollying mode: */ navigationMode=DOLLYING_WHEEL; } else { /* Calculate the scaling center: */ screenCenter=calcScreenCenter(); /* Initialize the wheel scaling factor: */ currentWheelScale=Scalar(1); preScale=NavTrackerState::translateFromOriginTo(screenCenter); postScale=NavTrackerState::translateToOriginFrom(screenCenter); postScale*=getNavigationTransformation(); /* Go to wheel scaling mode: */ navigationMode=SCALING_WHEEL; } } break; default: /* This can definitely happen; just ignore the event */ break; } } else { /* Act depending on this tool's current state: */ switch(navigationMode) { case DOLLYING_WHEEL: case SCALING_WHEEL: /* Deactivate this tool: */ deactivate(); /* Go to idle mode: */ navigationMode=IDLE; break; default: /* This can definitely happen; just ignore the event */ break; } } }
void MouseDialogNavigationTool::buttonCallback(int,InputDevice::ButtonCallbackData* cbData) { if(cbData->newButtonState) // Button has just been pressed { bool takeEvent=true; if(factory->interactWithWidgets) { /* Check if the GUI interactor accepts the event: */ GUIInteractor::updateRay(); if(GUIInteractor::buttonDown(false)) { /* Deactivate this tool if it is spinning: */ if(spinning) deactivate(); spinning=false; /* Disable navigation: */ takeEvent=false; } } if(takeEvent) { /* Deactivate spinning: */ spinning=false; /* Start navigating according to the current navigation mode: */ switch(navigationMode) { case ROTATING: if(activate()) startRotating(); break; case PANNING: if(activate()) startPanning(); break; case DOLLYING: if(activate()) startDollying(); break; case SCALING: if(activate()) startScaling(); break; } } } else // Button has just been released { if(GUIInteractor::isActive()) { /* Deliver the event: */ GUIInteractor::buttonUp(); } else { /* Check for spinning if currently in rotating mode: */ if(navigationMode==ROTATING) { /* Check if the input device is still moving: */ Point currentPos=calcScreenPos(); Vector delta=currentPos-lastRotationPos; if(Geometry::mag(delta)>factory->spinThreshold) { /* Calculate spinning angular velocity: */ Vector offset=(lastRotationPos-screenCenter)+rotateOffset; Vector axis=offset^delta; Scalar angularVelocity=Geometry::mag(delta)/(factory->rotateFactor*(getApplicationTime()-lastMoveTime)); spinAngularVelocity=axis*(Scalar(0.5)*angularVelocity/axis.mag()); /* Enable spinning: */ spinning=true; } else { /* Deactivate the tool: */ deactivate(); } } else { /* Deactivate the tool: */ deactivate(); } } } }
void * handleClient(void *fd) { int cliFd = (int)fd; int recvRes, sendRes; char *username, *password; username = malloc(USERNAME_SIZE); password = malloc(USERNAME_SIZE); while(1) { if((recvRes = recv(cliFd, username, USERNAME_SIZE, 0)) <= 0 ) { perror("recv"); close(cliFd); pthread_exit(NULL); } if((recvRes = recv(cliFd, password, USERNAME_SIZE, 0)) <= 0 ) { perror("recv"); close(cliFd); pthread_exit(NULL); } int loginRes; if((loginRes = login(cliFd, username, password)) < 0) { char *msg = "Incorrect password\n"; if((sendRes = send(cliFd, msg, strlen(msg), 0)) < 0 ) { perror("send"); close(cliFd); pthread_exit(NULL); } } else { char *msg = "Login Success\n"; if((sendRes = send(cliFd, msg, strlen(msg), 0)) < 0 ) { perror("send"); close(cliFd); pthread_exit(NULL); } break; } } broadcast(username, "has connected!"); //inform everyone that the new client has connected showActiveUsers(cliFd, username); char *buf; buf = malloc(BUFFER_SIZE); while(1) { //keep listening for stuff that the client has to say recvRes=0; bzero(buf, BUFFER_SIZE); if((recvRes = recv(cliFd, buf, BUFFER_SIZE, 0)) < 0) { //if there's some error in receiving, remove client and close connection perror("recv"); broadcast(username, "has disconnected"); deactivate(cliFd); close(cliFd); pthread_exit(NULL); } else if(recvRes == 0) { //client disconnected. remove client and close connection broadcast(username, "has disconnected"); deactivate(cliFd); close(cliFd); pthread_exit(NULL); } int sendchatRes; if((sendchatRes = (sendchat(username, buf))) < 0) { if(sendchatRes == -1) strcpy(buf, "User is currently not online\n"); else strcpy(buf, "User doesnt exist\n"); if(send(cliFd, buf, strlen(buf), 0) < 0) { perror("send"); broadcast(username, "has disconnected"); deactivate(cliFd); close(cliFd); pthread_exit(NULL); } } } }
TaskQueue::~TaskQueue() { deactivate(); }
unsigned int enemy::destiny_reached() { deactivate(); return 1; }
void enemy::update() { if(spawned()) { if(alive()) { if(!idle()) { //not idle option position=movement_update(position,destiny,speed); } else set_to_idle(); //if reach destiny } else if(current_animation!=dead_anim) kill(); //killed animation[current_animation].update(); //animation update if(current_animation==dead_anim && animation[current_animation].is_active()==false) deactivate(); } }
void MouseNavigationTool::buttonCallback(int,int buttonIndex,InputDevice::ButtonCallbackData* cbData) { /* Process based on which button was pressed: */ switch(buttonIndex) { case 0: if(cbData->newButtonState) // Button has just been pressed { /* Act depending on this tool's current state: */ switch(navigationMode) { case IDLE: case SPINNING: if(factory->interactWithWidgets) { /* Check if the mouse pointer is over a GLMotif widget: */ GLMotif::Event event(false); event.setWorldLocation(calcSelectionRay()); if(getWidgetManager()->pointerButtonDown(event)) { if(navigationMode==SPINNING) { /* Deactivate this tool: */ deactivate(); } /* Go to widget interaction mode: */ navigationMode=WIDGETING; /* Drag the entire root widget if the event's target widget is a title bar: */ if(dynamic_cast<GLMotif::TitleBar*>(event.getTargetWidget())!=0) { /* Start dragging: */ draggedWidget=event.getTargetWidget(); /* Calculate the dragging transformation: */ NavTrackerState initialTracker=NavTrackerState::translateFromOriginTo(calcScreenPos()); preScale=Geometry::invert(initialTracker); GLMotif::WidgetManager::Transformation initialWidget=getWidgetManager()->calcWidgetTransformation(draggedWidget); preScale*=NavTrackerState(initialWidget); } else draggedWidget=0; } else { /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) startRotating(); } } else { /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) startRotating(); } break; case PANNING: if(dolly) startDollying(); else startScaling(); break; default: /* This shouldn't happen; just ignore the event */ break; } } else // Button has just been released { /* Act depending on this tool's current state: */ switch(navigationMode) { case WIDGETING: { /* Deliver the event: */ GLMotif::Event event(true); event.setWorldLocation(calcSelectionRay()); getWidgetManager()->pointerButtonUp(event); /* Deactivate this tool: */ navigationMode=IDLE; draggedWidget=0; break; } case ROTATING: { /* Check if the input device is still moving: */ Point currentPos=calcScreenPos(); Vector delta=currentPos-lastRotationPos; if(Geometry::mag(delta)>factory->spinThreshold) { /* Calculate spinning angular velocity: */ Vector offset=(lastRotationPos-screenCenter)+rotateOffset; Vector axis=Geometry::cross(offset,delta); Scalar angularVelocity=Geometry::mag(delta)/(factory->rotateFactor*getCurrentFrameTime()); spinAngularVelocity=axis*(Scalar(0.5)*angularVelocity/axis.mag()); /* Go to spinning mode: */ navigationMode=SPINNING; } else { /* Deactivate this tool: */ deactivate(); /* Go to idle mode: */ navigationMode=IDLE; } break; } case DOLLYING: case SCALING: startPanning(); break; default: /* This shouldn't happen; just ignore the event */ break; } } break; case 1: if(cbData->newButtonState) // Button has just been pressed { /* Act depending on this tool's current state: */ switch(navigationMode) { case IDLE: case SPINNING: /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) startPanning(); break; case ROTATING: if(dolly) startDollying(); else startScaling(); break; default: /* This shouldn't happen; just ignore the event */ break; } } else // Button has just been released { /* Act depending on this tool's current state: */ switch(navigationMode) { case PANNING: /* Deactivate this tool: */ deactivate(); /* Go to idle mode: */ navigationMode=IDLE; break; case DOLLYING: case SCALING: startRotating(); break; default: /* This shouldn't happen; just ignore the event */ break; } } break; case 2: /* Set the dolly flag: */ dolly=cbData->newButtonState; if(factory->invertDolly) dolly=!dolly; if(dolly) // Dollying has just been enabled { /* Act depending on this tool's current state: */ switch(navigationMode) { case SCALING: startDollying(); break; default: /* Nothing to do */ break; } } else { /* Act depending on this tool's current state: */ switch(navigationMode) { case DOLLYING: startScaling(); break; default: /* Nothing to do */ break; } } break; } }
void MouseDialogNavigationTool::buttonCallback(int,int buttonIndex,InputDevice::ButtonCallbackData* cbData) { if(cbData->newButtonState) // Button has just been pressed { /* Deactivate spinning: */ spinning=false; /* Start navigating according to the current navigation mode: */ switch(navigationMode) { case ROTATING: if(activate()) startRotating(); break; case PANNING: if(activate()) startPanning(); break; case DOLLYING: if(activate()) startDollying(); break; case SCALING: if(activate()) startScaling(); break; } } else // Button has just been released { /* Check for spinning if currently in rotating mode: */ if(navigationMode==ROTATING) { /* Check if the input device is still moving: */ Point currentPos=calcScreenPos(); Vector delta=currentPos-lastRotationPos; if(Geometry::mag(delta)>factory->spinThreshold) { /* Calculate spinning angular velocity: */ Vector offset=(lastRotationPos-screenCenter)+rotateOffset; Vector axis=Geometry::cross(offset,delta); Scalar angularVelocity=Geometry::mag(delta)/(factory->rotateFactor*getFrameTime()); spinAngularVelocity=axis*(Scalar(0.5)*angularVelocity/axis.mag()); /* Enable spinning: */ spinning=true; } else { /* Deactivate the tool: */ deactivate(); } } else { /* Deactivate the tool: */ deactivate(); } } }
void CEventLoop::processActivity(int32_t command) { switch (command) { case APP_CMD_CONFIG_CHANGED: m_activityHandler->onConfigurationChanged(); break; case APP_CMD_INIT_WINDOW: // The window is being shown, get it ready. m_activityHandler->onCreateWindow(m_application->window); // set the proper DPI value m_inputHandler->setDPI(CXBMCApp::GetDPI()); break; case APP_CMD_WINDOW_RESIZED: // The window has been resized m_activityHandler->onResizeWindow(); break; case APP_CMD_TERM_WINDOW: // The window is being hidden or closed, clean it up. m_activityHandler->onDestroyWindow(); deactivate(); break; case APP_CMD_GAINED_FOCUS: activate(); m_activityHandler->onGainFocus(); break; case APP_CMD_LOST_FOCUS: m_activityHandler->onLostFocus(); deactivate(); break; case APP_CMD_LOW_MEMORY: m_activityHandler->onLowMemory(); break; case APP_CMD_START: m_activityHandler->onStart(); break; case APP_CMD_RESUME: m_activityHandler->onResume(); break; case APP_CMD_SAVE_STATE: // The system has asked us to save our current state. Do so. m_activityHandler->onSaveState(&m_application->savedState, &m_application->savedStateSize); break; case APP_CMD_PAUSE: m_activityHandler->onPause(); deactivate(); break; case APP_CMD_STOP: m_activityHandler->onStop(); break; case APP_CMD_DESTROY: m_activityHandler->onDestroy(); break; default: break; } }
MapUpdater::~MapUpdater() { deactivate(); }
bool SpecificWorker::deactivateAgent() { return deactivate(); }
//! [3] MceVibrator::~MceVibrator() { deactivate(lastPatternName); QDBusMessage reply = mceInterface.call(MCE_DISABLE_VIBRATOR); checkError(reply); }
void MapUpdater::ReActivate(uint32 threads) { deactivate(); activate(threads); SetBroken(false); }
void MouseNavigationTool::buttonCallback(int buttonSlotIndex,InputDevice::ButtonCallbackData* cbData) { /* Process based on which button was pressed: */ switch(buttonSlotIndex) { case 0: if(cbData->newButtonState) // Button has just been pressed { /* Act depending on this tool's current state: */ switch(navigationMode) { case IDLE: case SPINNING: if(factory->interactWithWidgets) { /* Check if the GUI interactor accepts the event: */ GUIInteractor::updateRay(); if(GUIInteractor::buttonDown(false)) { /* Deactivate this tool if it is spinning: */ if(navigationMode==SPINNING) deactivate(); /* Go to widget interaction mode: */ navigationMode=WIDGETING; } else { /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) startRotating(); } } else { /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) startRotating(); } break; case PANNING: if(dolly) startDollying(); else startScaling(); break; default: /* This shouldn't happen; just ignore the event */ break; } } else // Button has just been released { /* Act depending on this tool's current state: */ switch(navigationMode) { case WIDGETING: { if(GUIInteractor::isActive()) { /* Deliver the event: */ GUIInteractor::buttonUp(); } /* Deactivate this tool: */ navigationMode=IDLE; break; } case ROTATING: { /* Check if the input device is still moving: */ Point currentPos=calcScreenPos(); Vector delta=currentPos-lastRotationPos; if(Geometry::mag(delta)>factory->spinThreshold) { /* Calculate spinning angular velocity: */ Vector offset=(lastRotationPos-screenCenter)+rotateOffset; Vector axis=Geometry::cross(offset,delta); Scalar angularVelocity=Geometry::mag(delta)/(factory->rotateFactor*(getApplicationTime()-lastMoveTime)); spinAngularVelocity=axis*(Scalar(0.5)*angularVelocity/axis.mag()); /* Go to spinning mode: */ navigationMode=SPINNING; } else { /* Deactivate this tool: */ deactivate(); /* Go to idle mode: */ navigationMode=IDLE; } break; } case DOLLYING: case SCALING: startPanning(); break; default: /* This shouldn't happen; just ignore the event */ break; } } break; case 1: if(cbData->newButtonState) // Button has just been pressed { /* Act depending on this tool's current state: */ switch(navigationMode) { case IDLE: case SPINNING: /* Try activating this tool: */ if(navigationMode==SPINNING||activate()) startPanning(); break; case ROTATING: if(dolly) startDollying(); else startScaling(); break; default: /* This shouldn't happen; just ignore the event */ break; } } else // Button has just been released { /* Act depending on this tool's current state: */ switch(navigationMode) { case PANNING: /* Deactivate this tool: */ deactivate(); /* Go to idle mode: */ navigationMode=IDLE; break; case DOLLYING: case SCALING: startRotating(); break; default: /* This shouldn't happen; just ignore the event */ break; } } break; case 2: /* Set the dolly flag: */ dolly=cbData->newButtonState; if(factory->invertDolly) dolly=!dolly; if(dolly) // Dollying has just been enabled { /* Act depending on this tool's current state: */ switch(navigationMode) { case SCALING: startDollying(); break; default: /* Nothing to do */ break; } } else { /* Act depending on this tool's current state: */ switch(navigationMode) { case DOLLYING: startScaling(); break; default: /* Nothing to do */ break; } } break; } }
void CCSceneObject::removeFromScene() { deactivate(); inScene->removeObject( this ); }
void waypoints_follower::run() { const double MAX_TWIST_LINEAR = 1.0; const double MAX_TWIST_ANGULAR = 0.5; const double TURNING_RADIUS=0.4; if (!active) return; // ROS_INFO("active"); if (!localized) { ROS_WARN("waiting for localization"); return; } if (reached(next_target) || start_new_target) //Change of target! { start_new_target = false; if (targets.size()==0) { deactivate(deactivate_reason::NO_MORE_TARGETS); return; } std::unique_lock<std::mutex>(targets_mtx); next_target = targets.front(); targets.pop_front(); geometry_msgs::Pose2D current_pose; current_pose.x=x; current_pose.y=y; xtarget = next_target.x; ytarget = next_target.y; ROS_INFO_STREAM("starting new target "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000); straight=true; turning=false; } /* More complex implementation, for the future, it chooses speed and angular radius if (distance(next_target)<TURNING_RADIUS && !turning && targets.size()>0 && !start_new_target) //Will not use circle if this is the last target or if we are starting now { //TODO start turning along a circle //Find next heading, circle radius, circle length, circle angle double current_heading=theta; double heading=atan2(ytarget-targets.front().y,xtarget-targets.front().x); double current_speed=twist.linear.x; double next_speed=distance(next_target,targets.front())/(targets.front().z-next_target.z); double delta_heading=heading-current_heading; bool rotate_left; if((0<delta_heading && delta_heading<M_PI) || (-2*M_PI<delta_heading && delta_heading<-M_PI )) rotate_left=true; else rotate_left=false; bool ok=false; double desired_speed=(current_speed+next_speed)/2.0; double radius=max_turning_radius/10.0; double alpha=2*radius/wheel_distance; while(!ok) { double right_wheel_speed=-desired_speed*(1/alpha+1); if (right_wheel_speed>max_speed) //Too fast, slow down { right_wheel_speed=max_speed; } double left_wheel_speed=2*desired_speed-right_wheel_speed; if (left_wheel_speed>max_speed) //Also too fast, change speed and turning radius { desired_speed=0.9*desired_speed; radius=radius+max_turning_radius*0.1; } else { ok=true; } } turning = true; twist.linear.x=desired_speed; twist.angular.z=(right_wheel_speed-left_wheel_speed)/2.0; } */ if (distance(next_target)<TURNING_RADIUS && distance(next_target)>reached_threshold && !turning && targets.size()>0 && !start_new_target) //Will not use circle if this is the last target or if we are starting now { turning=true; straight=false; desired_heading=atan2(targets.front().y-ytarget,targets.front().x-xtarget); double current_speed=twist.linear.x; double next_speed=distance(next_target,targets.front())/(targets.front().z-next_target.z); if(desired_speed>MAX_TWIST_LINEAR) desired_speed=MAX_TWIST_LINEAR; desired_speed=(current_speed+next_speed)/2.0; twist.linear.x=desired_speed; ROS_INFO_STREAM("starting turning from near "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000<< "to next target"); } double length = distance(next_target); double theta_err; if (turning) { //either keep publishing same rotation and speed or feedback on the circle information twist.linear.x=desired_speed*(1+ki1*(TURNING_RADIUS-length)); twist.angular.z=-ki2*sin(desired_heading-theta); theta_err=desired_heading-theta; if (fabs(desired_heading-theta)<0.2 )//|| distance()) { //We kind of reached the desired heading, we should switch to the next target and stop turning std::unique_lock<std::mutex>(targets_mtx); next_target = targets.front(); targets.pop_front(); geometry_msgs::Pose2D current_pose; current_pose.x=x; current_pose.y=y; xtarget = next_target.x; ytarget = next_target.y; straight=true; ROS_INFO_STREAM("starting new target "<<xtarget<< " " <<ytarget<<" "<<int(next_target.z)%1000); ROS_INFO("starting straight"); turning=false; } } if (straight) { double delta = next_target.z - ros::Time::now().toSec(); if (delta<-0) { twist.linear.x = 0; ROS_WARN_STREAM("Target time is in the past, stopping"); deactivate(deactivate_reason::GLOBAL_TARGET_NOT_REACHED); } else { twist.linear.x=length/delta*1.1; theta_err=atan2(ytarget-y,xtarget-x)-theta; if (fabs(fabs(theta_err)-M_PI)<0.1) theta_err=theta_err+0.1; twist.angular.z=-kp2*sin(theta_err); } } twist.linear.x = std::min(twist.linear.x,MAX_TWIST_LINEAR); comand_pub.publish(twist); ROS_DEBUG_STREAM("controller run xt: "<<xtarget<<" yt: "<<ytarget<<" x: "<<x<<" y: "<<y<<" t "<<next_target.z); ROS_DEBUG_STREAM("controller run theta:"<<theta<<" error "<<theta_err<<" v: "<<twist.linear.x<<" w: "<<twist.angular.z); ros::spinOnce(); }
bool ParameterFileModel::setData( const QModelIndex& ind, const QVariant& value, int role) { if (!prefixValid()) return false; switch (role) { case Qt::EditRole: case Qt::DisplayRole: if ((ind.row() >= 0) && (ind.row() < _keys.size())) { switch (ind.column()) { case 0: if (_onlyParams) return false; if (value.canConvert(QVariant::String)) { QString oldName = _keys[ind.row()]; QString newName; // do not forget the prefix at the beginning of the name if (!_prefix.isEmpty()) newName = _prefix + "."; newName += value.toString(); if (oldName == newName) return true; // nothing to do if (_parameterFile->isSet(newName)) return false; // don't overwrite existing value // save value QString val = getValue(oldName); erase(oldName); setValue(newName, val); emit dataChanged(ind, ind); return true; } break; case 1: if (value.canConvert(QVariant::String)) { QString valueStr = value.toString(); QString keyStr = _keys[ind.row()]; if (valueStr == getValue(keyStr)) return true; // nothing to do setValue(keyStr, valueStr); if (_onlyParams && _metaInfos->isDynamic(getClass(keyStr))) { save(); _updateDynamics(); QTimer::singleShot(0, this, SLOT(_update())); emit dynamicUpdate(); } emit dataChanged(index(ind.row(), 0), ind); return true; } break; case 2: if (value.canConvert(QVariant::Int)) { // check if value is allowed int valueInt = value.toInt(); QString valueStr = QVariant(valueInt).toString(); if (valueInt < 0 || valueInt > 3) { return false; } if (valueInt == 0) { // 0 is default value -> no entry needed if (isSet(_keys[ind.row()] + ".editorpriority")) { // check if value exists to prevent exceptions erase(_keys[ind.row()] + ".editorpriority"); emit dataChanged(index(ind.row(), 0), ind); } return true; } if (valueStr == getValue(_keys[ind.row()] + ".editorpriority")) { return true; // nothing to do } setValue(_keys[ind.row()] + ".editorpriority", valueStr); emit dataChanged(index(ind.row(), 0), ind); return true; } break; } } break; case Qt::CheckStateRole: if (_useMetaInfo && ind.column()==1 && isParameter(_keys[ind.row()])) { Q_ASSERT(getType(_keys[ind.row()]) == "bool"); bool checked (value.toBool()); setValue (_keys[ind.row()], checked ? "true" : "false"); emit dataChanged(index(ind.row(),0),ind); if (_keys[ind.row()].contains("active") && checked == true){ reactivatePreviousPlugins(); } // additional check whether the changed Parameter is the ActiveInactive if (_keys[ind.row()].contains("active") && checked == false){ deactivate(); } } break; } return false; }