void
PlanConfigParser::parseActions(Parsers::Config& cfg,
const std::vector<std::string>& sections,
IMC::MessageList<IMC::Message>& actions)
{
for (unsigned i = 0; i < sections.size(); ++i)
{
std::vector<std::string> options;
options = cfg.options(sections[i]);
IMC::SetEntityParameters sep;
cfg.get(sections[i], "Name", "", sep.name);
for (unsigned j = 0; j < options.size(); ++j)
{
if (options[j] == "Name")
continue;
IMC::EntityParameter ent_par;
ent_par.name = options[j];
cfg.get(sections[i], ent_par.name, "", ent_par.value);
sep.params.push_back(ent_par);
}
actions.push_back(sep);
}
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string section, IMC::LblBeaconSetup& bs)
{
bs.beacon = section;
parseCoordinate(cfg, section, bs);
parseZ(cfg, section);
cfg.get(section, "Transponder Delay (msecs)", "", bs.transponder_delay);
cfg.get(section, "Interrogation Channel", "", bs.query_channel);
cfg.get(section, "Reply Channel", "", bs.reply_channel);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::PopUp& man)
{
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseDuration(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
cfg.get(id, "Radius (meters)", "15.0", man.radius);
cfg.get(id, "Flags", "0x00", man.flags);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string section, IMC::Elevator& man)
{
parseSpeed(cfg, section, man);
parseCoordinate(cfg, section, man);
cfg.get(section, "Flags", "0x00", man.flags);
parseZUnits(cfg, section, man.start_z_units, "Start Z Units");
parseZUnits(cfg, section, man.end_z_units, "End Z Units");
cfg.get(section, "Start Z (meters)", "0.0", man.start_z);
cfg.get(section, "End Z (meters)", "0.0", man.end_z);
cfg.get(section, "Radius (meters)", "15.0", man.radius);
}
static void
parseSpeed(Parsers::Config& parser, std::string section, T& man)
{
std::string u;
parser.get(section, "Speed", "100.0", man.speed);
parser.get(section, "Speed Units", "%", u);
if (u == "m/s")
man.speed_units = IMC::SUNITS_METERS_PS;
else if (u == "rpm")
man.speed_units = IMC::SUNITS_RPM;
else
man.speed_units = IMC::SUNITS_PERCENTAGE;
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::FollowPath& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
int n_points;
cfg.get(id, "Number of Points", "0", n_points);
Math::Matrix W(n_points, 3);
W.readFromConfig(cfg, id, "Points");
IMC::MessageList<IMC::PathPoint>* list = &man.points;
for (int i = 0; i < W.rows(); ++i)
{
IMC::PathPoint* p = new IMC::PathPoint;
p->x = W(i, 0);
p->y = W(i, 1);
p->z = W(i, 2);
list->push_back(*p);
}
}
inline static void
parseCoordinate(Parsers::Config& parser, std::string section, T& msg)
{
std::string ref;
parser.get(section, "Reference", section, ref);
parseAngle(parser, ref, "Latitude (degrees)", msg.lat, 0.0);
parseAngle(parser, ref, "Longitude (degrees)", msg.lon, 0.0);
// See if there are N-E offsets
double n, e;
parser.get(section, "Offset North (meters)", "0.0", n);
parser.get(section, "Offset East (meters)", "0.0", e);
Coordinates::WGS84::displace(n, e, &msg.lat, &msg.lon);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string section, IMC::YoYo& man)
{
parseCoordinate(cfg, section, man);
parseZ(cfg, section, man);
parseZUnits(cfg, section, man);
cfg.get(section, "Amplitude (meters)", "0.0", man.amplitude);
parseAngle(cfg, section, "Pitch (degrees)", man.pitch, (fp32_t)15.0);
parseSpeed(cfg, section, man);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::StationKeeping& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
parseDuration(cfg, id, man);
cfg.get(id, "Radius (meters)", "15.0", man.radius);
}
void
validateEntityLabels(Parsers::Config& cfg)
{
// Get all sections.
std::vector<std::string> vec = cfg.sections();
for (unsigned int i = 0; i < vec.size(); ++i)
{
if (vec[i].find_first_of(".") == std::string::npos)
continue;
std::string enabled;
cfg.get(vec[i], "Enabled", "Never", enabled);
if (enabled == "Never")
continue;
std::string entity_label;
cfg.get(vec[i], "Entity Label", "", entity_label);
if (entity_label.empty())
throw std::runtime_error(String::str("section '%s' doesn't have entity label", vec[i].c_str()));
}
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Loiter& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseDuration(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
int8_t type;
cfg.get(id, "Loiter Type", "0", type);
switch (type)
{
case 1:
man.type = IMC::Loiter::LT_RACETRACK; break;
case 2:
man.type = IMC::Loiter::LT_HOVER; break;
case 3:
man.type = IMC::Loiter::LT_EIGHT; break;
case 0:
default:
man.type = IMC::Loiter::LT_CIRCULAR; break;
}
std::string ldir;
cfg.get(id, "Loiter Direction", "Clock", ldir);
if (ldir == "Clockwise")
man.direction = IMC::Loiter::LD_CLOCKW;
else
man.direction = IMC::Loiter::LD_CCLOCKW;
cfg.get(id, "Radius (meters)", "50", man.radius);
parseAngle(cfg, id, "Bearing (degrees)", man.bearing, 0.0);
cfg.get(id, "Length (meters)", "100", man.length);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::CompassCalibration& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseTimeout(cfg, id, man);
parseDuration(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
std::string ldir;
cfg.get(id, "Loiter Direction", "Clock", ldir);
if (ldir == "Clockwise")
man.direction = IMC::Loiter::LD_CLOCKW;
else
man.direction = IMC::Loiter::LD_CCLOCKW;
cfg.get(id, "Radius (meters)", "50", man.radius);
cfg.get(id, "Amplitude (meters)", "1", man.amplitude);
parseAngle(cfg, id, "Pitch (degrees)", man.pitch, (fp32_t)0.0);
}
//! Parse z units specification for a maneuver.
//! @param[in] parser configuration parser handle.
//! @param[in] section configuration section.
//! @param[out] zunits destination variable.
//! @param[in] label label.
inline static void
parseZUnits(Parsers::Config& parser, std::string section, uint8_t& zunits, std::string label = "Z Units")
{
std::string z_str;
parser.get(section, label, "DEPTH", z_str);
if (std::strcmp(z_str.c_str(), "NONE") == 0)
zunits = IMC::Z_NONE;
else if (std::strcmp(z_str.c_str(), "DEPTH") == 0)
zunits = IMC::Z_DEPTH;
else if (std::strcmp(z_str.c_str(), "ALTITUDE") == 0)
zunits = IMC::Z_ALTITUDE;
else if (std::strcmp(z_str.c_str(), "HEIGHT") == 0)
zunits = IMC::Z_HEIGHT;
else
throw std::runtime_error(Utils::String::str(DTR("invalid z unit: %s"), z_str.c_str()));
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Rows& man)
{
// Get configurable parameters
parseCoordinate(cfg, id, man);
parseSpeed(cfg, id, man);
parseZ(cfg, id, man);
parseZUnits(cfg, id, man);
parseAngle(cfg, id, "Bearing (degrees)", man.bearing, 0.0);
parseAngle(cfg, id, "Cross Angle (degrees)", man.cross_angle, 0.0);
cfg.get(id, "Width (meters)", "150", man.width);
cfg.get(id, "Length (meters)", "100", man.length);
cfg.get(id, "Curve Offset (meters)", "15", man.coff);
cfg.get(id, "Alternation (%)", "100", man.alternation);
cfg.get(id, "Horizontal Step (meters)", "30", man.hstep);
cfg.get(id, "Flags", "3", man.flags);
}
inline static void
parseZ(Parsers::Config& parser, std::string section, T& man)
{
parser.get(section, "Z (meters)", "1.0", man.z);
}
inline static void
parseDuration(Parsers::Config& parser, std::string section, T& man)
{
parser.get(section, "Duration (seconds)", "0", man.duration);
}
inline static void
parseTimeout(Parsers::Config& parser, std::string section, T& man)
{
parser.get(section, "Timeout (seconds)", "0", man.timeout);
}
inline static void
parseAngle(Parsers::Config& parser, std::string section, std::string key, T& value, T dfl)
{
parser.get(section, key, DUNE::uncastLexical(dfl), value);
value = Math::Angles::radians(value);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Teleoperation& man)
{
cfg.get(id, "Custom Settings", "", man.custom);
}
void
PlanConfigParser::parse(Parsers::Config& cfg, IMC::PlanSpecification& plan)
{
cfg.get("Plan Configuration", "Plan ID", "", plan.plan_id);
// Parse plan actions
std::vector<std::string> plan_start_actions;
std::vector<std::string> plan_end_actions;
cfg.get("Plan Configuration", "Start Actions", "", plan_start_actions);
cfg.get("Plan Configuration", "End Actions", "", plan_end_actions);
parseActions(cfg, plan_start_actions, plan.start_actions);
parseActions(cfg, plan_end_actions, plan.end_actions);
std::vector<std::string> ids;
cfg.get("Plan Configuration", "Maneuvers", "", ids);
for (unsigned i = 0; i < ids.size(); i++)
{
std::string id = ids[i];
IMC::PlanManeuver pman;
pman.maneuver_id = id;
std::string type;
cfg.get(id, "Type", "!!unknown!!", type);
#ifdef DUNE_IMC_POPUP
if (type == "PopUp")
{
IMC::PopUp popup;
parse(cfg, id, popup);
pman.data.set(popup);
}
else
#endif
#ifdef DUNE_IMC_GOTO
if (type == "Goto")
{
IMC::Goto goto_man;
parse(cfg, id, goto_man);
pman.data.set(goto_man);
}
else
#endif
#ifdef DUNE_IMC_STATIONKEEPING
if (type == "StationKeeping")
{
IMC::StationKeeping sk_man;
parse(cfg, id, sk_man);
pman.data.set(sk_man);
}
else
#endif
#ifdef DUNE_IMC_IDLEMANEUVER
if (type == "Idle")
{
IMC::IdleManeuver idle;
parse(cfg, id, idle);
pman.data.set(idle);
}
else
#endif
#ifdef DUNE_IMC_LOITER
if (type == "Loiter")
{
IMC::Loiter loiter;
parse(cfg, id, loiter);
pman.data.set(loiter);
}
else
#endif
#ifdef DUNE_IMC_FOLLOWPATH
if (type == "FollowPath")
{
IMC::FollowPath fp;
parse(cfg, id, fp);
pman.data.set(fp);
}
else
#endif
#ifdef DUNE_IMC_ROWS
if (type == "Rows")
{
IMC::Rows r;
parse(cfg, id, r);
pman.data.set(r);
r.toText(std::cerr);
}
else
#endif
#ifdef DUNE_IMC_ELEMENTALMANEUVER
if (type == "ElementalManeuver")
{
IMC::ElementalManeuver eman;
parse(cfg, id, eman);
pman.data.set(eman);
}
else
#endif
#ifdef DUNE_IMC_YOYO
if (type == "YoYo")
{
IMC::YoYo yoyo;
parse(cfg, id, yoyo);
pman.data.set(yoyo);
}
else
#endif
#ifdef DUNE_IMC_ELEVATOR
if (type == "Elevator")
{
IMC::Elevator elev;
parse(cfg, id, elev);
pman.data.set(elev);
elev.toText(std::cerr);
}
else
#endif
#ifdef DUNE_IMC_DUBIN
if (type == "Dubin")
{
IMC::Dubin dub;
parse(cfg, id, dub);
pman.data.set(dub);
}
else
#endif
#ifdef DUNE_IMC_COMPASSCALIBRATION
if (type == "CompassCalibration")
{
IMC::CompassCalibration ccalib;
parse(cfg, id, ccalib);
pman.data.set(ccalib);
}
else
#endif
{
DUNE_ERR
("Plan Load", "Unknown or unsupported maneuver type: '" << type << '\'');
return;
}
// Parse maneuver actions
std::vector<std::string> man_start_actions;
std::vector<std::string> man_end_actions;
cfg.get(id, "Start Actions", "", man_start_actions);
cfg.get(id, "End Actions", "", man_end_actions);
parseActions(cfg, man_start_actions, pman.start_actions);
parseActions(cfg, man_end_actions, pman.end_actions);
plan.maneuvers.push_back(pman);
if (plan.maneuvers.size() == 1)
{
plan.start_man_id = id;
}
else
{
// See maneuver sequence in graph terms
IMC::PlanTransition ptransition;
ptransition.source_man = ids[plan.maneuvers.size() - 2];
ptransition.dest_man = id;
plan.transitions.push_back(ptransition);
}
}
}