bool angleIsBetween(const double p_angle, const double p_begin, const double p_end)
{
double angle = normalizeRadian(p_angle);
double begin = normalizeRadian(p_begin);
double end = normalizeRadian(p_end);
// overflow
if(end < begin) {
return begin <= angle || angle <= end;
} else {
return begin <= angle && angle <= end;
}
}
std::vector<SensorSetting> loadSensorSettings(const std::string &p_modelName)
{
if(!ros::master::check())
throw std::logic_error("ROS master is not running. Cannot load sensor settings");
std::vector<SensorSetting> result;
int i = 0;
while(ros::param::has(PARAM_SENSOR(p_modelName, i)))
{
result.push_back(SensorSetting());
result.back().id = i;
// get direction and convert to radian
ros::param::get(PARAM_SENSOR_DIRECTION(p_modelName, i), result.back().direction);
result.back().direction = degreeToRadian(result.back().direction);
result.back().direction = normalizeRadian(result.back().direction);
// get x and y positions relative to robot
ros::param::get(PARAM_SENSOR_X(p_modelName, i), result.back().x);
ros::param::get(PARAM_SENSOR_Y(p_modelName, i), result.back().y);
++i;
}
return result;
}