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; } }