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