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