/** * return a score for trajectory traj */ virtual double scoreTrajectory(Trajectory &traj) { if(traj.getPointsSize()==0) return 0.0; double px, py, pth; traj.getPoint(0, px, py, pth); double dist_to_goal = hypot(px-goal_x_, py-goal_y_); if(dist_to_goal < .2){ return 0.0; } double start_diff; if(!getAngleDiff(px,py,pth,&start_diff)) return -4.0; if(fabs(start_diff) < max_trans_angle_){ return 0.0; } if(fabs(traj.xv_) > 0.0 || fabs(traj.yv_) > 0.0) return -3.0; else if( sign(start_diff) != sign(traj.thetav_) ) return -2.0; traj.getPoint(traj.getPointsSize()-1, px, py, pth); double end_diff; if(!getAngleDiff(px,py,pth,&end_diff)) return -1.0; return fabs(end_diff); }
//------------------------------------------------------------------------------------------------------------------------------------- // void Tweenable2D::tweenAngleTo( float _angleTarget, float _timeSecs, EasingEquations::EaseType _easeType, float _startDelay, bool _backWhenDone, bool _fireEventWhenDone ) { angleTweenProps.eventID = eventID; angleTweenProps.animType = TweenDoneEventArgs::ANIM_ANGLE; angleTweenProps.startTime = timer.elapsedSec() + _startDelay; angleTweenProps.endTime = angleTweenProps.startTime + _timeSecs; angleTweenProps.tweenTime = _timeSecs; angleTweenProps.easeType = _easeType; float tmpAngDiff = getAngleDiff( angle, _angleTarget ); angleTweenProps.startX = angle; angleTweenProps.endX = angle + tmpAngDiff; //angleTweenProps.endX = _angleTarget; angleTweenProps.targetValRef = ∠ angleTweenProps.active = true; angleTweenProps.backWhenDone = _backWhenDone; angleTweenProps.fireEventWhenDone = _fireEventWhenDone; }
void GoToSelectedBall::selectedBallCb(const geometry_msgs::PointConstPtr& selectedBallPose){ // ROS_INFO("selectedBallCb"); if(selectedBallPose->x == 1000 && selectedBallPose->y == 1000 && selectedBallPose->z == 1000 ){ isBallPoseSet = false; // ROS_INFO("ret 1"); return; } current_pose_.x = selectedBallPose->x; current_pose_.y = selectedBallPose->y; isBallPoseSet = true; float angleDiffRobotGoal = getAngleDiff()*180/(3.14); }
// called whenever a touch is moved. void MultiTouchMotionCB(s3ePointerTouchMotionEvent* event) { if(event->m_x < 0) return; CTouch* touch = GetTouch(event->m_TouchID); if (touch->gesture_type == DRAG_WORLD && touch->active) { touch->last_x = touch->x; touch->last_y = touch->y; touch->x = event->m_x; touch->y = event->m_y; worldScrollSpeed = getAngleDiff(touch); } else { touch->x = event->m_x; touch->y = event->m_y; } }
float getAngleDiff(CTouch* touch) { return getAngleDiff(touch->last_x, touch->last_y, touch->x, touch->y); }