void Robot::spin() {//S型过人
cv::Point2f robot_coord(x, y);
std::vector<cv::Point2f> ans = findMulBall();
cv::Point2f ball1 = ans[0];
cv::Point2f ball2 = ans[1];
//printf("%f %f\n %f %f\n", ball1.x, ball1.y, ball2.x, ball2.y);
float delta = 30;
float r12 = cal_distance(ball1, ball2) / 2;
float rspin = (delta * delta + r12 * r12) / (2 * delta);
rspin += 10;//误差修正
float disr1 = cal_distance(ball1, robot_coord);
int inrspin = 0;
while(inrspin < rspin)
inrspin++;
//printf("rspin:%f, disr1:%f, inrspin:%d\n", rspin, disr1, inrspin);
cv::Point2f rto1 = ball1 - robot_coord;
cv::Point2f b1to2 = ball2 - ball1;
float arc = asin(r12 / rspin);
cv::Point2f pturn1(ball1.x - b1to2.x * 0.5, ball1.y - b1to2.y *0.5);
moveTo(pturn1, 20);
cv::Point2f robotPosition(x,y);
cv::Point2f dir= ball1 - robotPosition;
float dir_len = length(dir);
if (dir_len <= 0)
return;
cv::Point2f new_dir = dir*(1/dir_len);
rotateTo(new_dir);
turnRight(arc * 180 / M_PI);
moveRotate(true, rspin, arc * 2 + 0.2);//误差修正
moveRotate(false, rspin, arc * 2 + 0.2);
}
void InteractiveMarkerControl::handleMouseMovement( ViewportMouseEvent& event )
{
// handle mouse movement
float width = event.viewport->getActualWidth() - 1;
float height = event.viewport->getActualHeight() - 1;
Ogre::Ray mouse_ray = event.viewport->getCamera()->getCameraToViewportRay( (event.x + .5) / width,
(event.y + .5) / height );
Ogre::Ray last_mouse_ray =
event.viewport->getCamera()->getCameraToViewportRay(
(event.last_x + .5) / width, (event.last_y + .5) / height);
//convert rays into reference frame
mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( mouse_ray.getOrigin() ) );
mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * mouse_ray.getDirection() );
last_mouse_ray.setOrigin( reference_node_->convertWorldToLocalPosition( last_mouse_ray.getOrigin() ) );
last_mouse_ray.setDirection( reference_node_->convertWorldToLocalOrientation( Ogre::Quaternion::IDENTITY ) * last_mouse_ray.getDirection() );
switch (interaction_mode_)
{
case visualization_msgs::InteractiveMarkerControl::MOVE_AXIS:
moveAxis( mouse_ray, event );
break;
case visualization_msgs::InteractiveMarkerControl::MOVE_PLANE:
movePlane( mouse_ray );
break;
case visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE:
moveRotate( mouse_ray );
break;
case visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS:
rotate(mouse_ray);
break;
default:
break;
}
}
