Point2f CameraProjections::RotateTowardHeading(Point2f in)
{
return RotateAroundPoint(in,-Radian2Degree(getHeading()));
}
double selectionswitchAngle(double MEASUREMENT)
{
FunctionsParameters key;
printf("Выберите величину, из которой Вы хотите произвести конвертирование:\n"
"1 - Секунды;\n"
"2 - Минуты;\n"
"3 - Градусы;\n"
"4 - Радианы;\n"
"5 - Грады;\n"
"6 - Секстанты;\n"
"7 - Румбы;\n\t");
key.input_choice = parsing_id(key.in);
while(key.input_choice >= 8 || key.input_choice == 0) {
fprintf(stderr, "\nВы выбрали величину, которой нет в списке доступных величин. \nПожалуйста, сделайте корректный выбор\n\t");
key.input_choice = parsing_id(key.in);
}
printf("Выберите величину, в которую Вы хотите конвертировать Вашу величину.\n"
"Не нужно выбирать ту же величину, что Вы выбрали в качестве конвертируемой:\n"
"1 - Секунды;\n"
"2 - Минуты;\n"
"3 - Градусы\n"
"4 - Радианы;\n"
"5 - Грады;\n"
"6 - Секстанты;\n"
"7 - Румбы;\n\t");
key.output_choice = parsing_id(key.out);
while(key.output_choice == key.input_choice || key.output_choice >= 8 || key.output_choice == 0 ) {
fprintf(stderr, "\nВы выбрали величину, которой нет в списке доступных величин, \nили Вы пытаетесь конвертировать "
"одну и ту же величину. \nПожалуйста, сделайте корректный выбор\n\t");
key.output_choice = parsing_id(key.out);
}
key.id = key.input_choice * 10 + key.output_choice;
switch(key.id) {
case 12:
key.gate = Degree2Min(Sec2Degree(MEASUREMENT));
break;
case 13:
key.gate = Sec2Degree(MEASUREMENT);
break;
case 14:
key.gate = Degree2Radian(Sec2Degree(MEASUREMENT));
break;
case 15:
key.gate = Degree2Grad(Sec2Degree(MEASUREMENT));
break;
case 16:
key.gate = Degree2Sextant(Sec2Degree(MEASUREMENT));
break;
case 17:
key.gate = Degree2Rumb(Sec2Degree(MEASUREMENT));
break;
case 21:
key.gate = Degree2Sec(Min2Degree(MEASUREMENT));
break;
case 23:
key.gate = Min2Degree(MEASUREMENT);
break;
case 24:
key.gate = Degree2Radian(Min2Degree(MEASUREMENT));
break;
case 25:
key.gate = Degree2Grad(Min2Degree(MEASUREMENT));
break;
case 26:
key.gate = Degree2Sextant(Min2Degree(MEASUREMENT));
break;
case 27:
key.gate = Degree2Rumb(Min2Degree(MEASUREMENT));
break;
case 31:
key.gate = Degree2Sec(MEASUREMENT);
break;
case 32:
key.gate = Degree2Min(MEASUREMENT);
break;
case 34:
key.gate = Degree2Radian(MEASUREMENT);
break;
case 35:
key.gate = Degree2Grad(MEASUREMENT);
break;
case 36:
key.gate = Degree2Sextant(MEASUREMENT);
break;
case 37:
key.gate = Degree2Rumb(MEASUREMENT);
break;
case 41:
key.gate = Degree2Sec(Radian2Degree(MEASUREMENT));
break;
case 42:
key.gate = Degree2Min(Radian2Degree(MEASUREMENT));
break;
case 43:
key.gate = Radian2Degree(MEASUREMENT);
break;
case 45:
key.gate = Degree2Grad(Radian2Degree(MEASUREMENT));
break;
case 46:
key.gate = Degree2Sextant(Radian2Degree(MEASUREMENT));
break;
case 47:
key.gate = Degree2Rumb(Radian2Degree(MEASUREMENT));
break;
case 51:
key.gate = Degree2Sec(Grad2Degree(MEASUREMENT));
break;
case 52:
key.gate = Degree2Min(Grad2Degree(MEASUREMENT));
break;
case 53:
key.gate = Grad2Degree(MEASUREMENT);
break;
case 54:
key.gate = Degree2Radian(Grad2Degree(MEASUREMENT));
break;
case 56:
key.gate = Degree2Sextant(Grad2Degree(MEASUREMENT));
break;
case 57:
key.gate = Degree2Rumb(Grad2Degree(MEASUREMENT));
break;
case 61:
key.gate = Degree2Sec(Sextant2Degree(MEASUREMENT));
break;
case 62:
key.gate = Degree2Min(Sextant2Degree(MEASUREMENT));
break;
case 63:
key.gate = Sextant2Degree(MEASUREMENT);
break;
case 64:
key.gate = Degree2Radian(Sextant2Degree(MEASUREMENT));
break;
case 65:
key.gate = Degree2Grad(Sextant2Degree(MEASUREMENT));
break;
case 67:
key.gate = Degree2Rumb(Sextant2Degree(MEASUREMENT));
break;
case 71:
key.gate = Degree2Sec(Rumb2Degree(MEASUREMENT));
break;
case 72:
key.gate = Degree2Min(Rumb2Degree(MEASUREMENT));
break;
case 73:
key.gate = Rumb2Degree(MEASUREMENT);
break;
case 74:
key.gate = Degree2Radian(Rumb2Degree(MEASUREMENT));
break;
case 75:
key.gate = Degree2Grad(Rumb2Degree(MEASUREMENT));
break;
case 76:
key.gate = Degree2Sextant(Rumb2Degree(MEASUREMENT));
break;
default:
printf("Не введено значение конвертируемой величины, "
"или Вы пытаетесь конвертировать в величину, которую уже преобразуете\n");
break;
}
return key.gate;
}
bool CameraProjections::Update()
{
try
{
diagnalAngleView = params.camera.diagonalAngleView->get();
if (dummy && (ros::Time::now() < lastTime))
{
ROS_WARN(
"Vision Module detected a backward time. Cleared all cached tf data.");
m_tf->clear();
delete m_tf;
m_tf = new tf::TransformListener(ros::Duration(10));
m_tf->waitForTransform("/ego_rot", "/camera_optical",
ros::Time::now()
- ros::Duration(params.debug.timeToShift->get()),
ros::Duration(1.0));
}
lastTime = ros::Time::now();
topViewMarker.clear();
ros::Time past = ros::Time::now() - ros::Duration(0.060); //To get the 40ms before
if (dummy)
{
past = ros::Time(0); //To get latest
}
m_tf->lookupTransform("/ego_rot", "/camera_optical",
ros::Time::now()
- ros::Duration(params.debug.timeToShift->get()),
tfOptical);
tfScalar roll, pitch, yaw;
tfOptical.getBasis().getEulerYPR(yaw, pitch, roll);
// cout<< " X = "<<tfOptical.getOrigin().getX() << " Y= "<< tfOptical.getOrigin().getY()<< " Z="<< tfOptical.getOrigin().getZ()<<endl;
OpticalAngle.x = pitch;
OpticalAngle.y = -(roll + M_PI);
OpticalAngle.z = yaw + M_PI / 2.;
if (VERBOSE)
{
ROS_INFO_THROTTLE(0.33, "x= %1.2f y= %1.2f z=%1.2f",
Radian2Degree(OpticalAngle.x),
Radian2Degree(OpticalAngle.y),
Radian2Degree(OpticalAngle.z));
}
cameraLocation.x = params.location.x->get(); //todo: it seems that the actual height published in odometry
cameraLocation.y = params.location.y->get();
cameraLocation.z = params.location.z->get();
cameraOrintation.x = OpticalAngle.x + params.orientation.x->get();
cameraOrintation.y = OpticalAngle.y + params.orientation.y->get();
cameraOrintation.z = CorrectAngleRadian360(
OpticalAngle.z + params.orientation.z->get());
} catch (tf::TransformException &ex)
{
std::string errorMsg(ex.what());
// if(errorMsg.find("Lookup would require extrapolation")!=std::string::npos)
// {
// reset_time_pub.publish(std_msgs::Empty());
// reset_clock_pub.publish(std_msgs::Empty());
// }
ROS_ERROR("%s", errorMsg.c_str());
return false;
}
return true;
}
