Пример #1
0
    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);
      }
    }
Пример #2
0
 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);
 }
Пример #3
0
 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);
 }
Пример #4
0
 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);
 }
Пример #5
0
 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;
 }
Пример #6
0
    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);
      }
    }
Пример #7
0
      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);
      }
Пример #8
0
 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);
 }
Пример #9
0
 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);
 }
Пример #10
0
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()));
  }
}
Пример #11
0
    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);
    }
Пример #12
0
    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);
    }
Пример #13
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()));
      }
Пример #14
0
 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);
 }
Пример #15
0
 inline static void
 parseZ(Parsers::Config& parser, std::string section, T& man)
 {
   parser.get(section, "Z (meters)", "1.0", man.z);
 }
Пример #16
0
 inline static void
 parseDuration(Parsers::Config& parser, std::string section, T& man)
 {
   parser.get(section, "Duration (seconds)", "0", man.duration);
 }
Пример #17
0
 inline static void
 parseTimeout(Parsers::Config& parser, std::string section, T& man)
 {
   parser.get(section, "Timeout (seconds)", "0", man.timeout);
 }
Пример #18
0
 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);
 }
Пример #19
0
 void
 PlanConfigParser::parse(Parsers::Config& cfg, std::string id, IMC::Teleoperation& man)
 {
   cfg.get(id, "Custom Settings", "", man.custom);
 }
Пример #20
0
    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);
        }
      }
    }