Example #1
0
/*
  \brief Another sanity and demo test. Shows the user how to interface with a CapnObject that uses many fields as well as shows that a standard type that is generated is working.
*/
void test_imu()
{
   ::capnp::MallocMessageBuilder imu_b;
   auto imu = imu_b.initRoot<gams::types::Imu>();

   auto lin_acc = imu.initLinearAccelerationCovariance(9);
   lin_acc.set(0, 0.01);
   lin_acc.set(1, 0.03);

   ::capnp::MallocMessageBuilder header_b;
   auto header = imu.initHeader();
   header.setStamp(10);
   header.setFrameId("world");
   header.setSeq(100);

   // Incorrect
   //imu.setHeader(header);
   //imu.setLinearAccelerationCovariance(lin_acc.asReader());

   k.set_any("imu", madara::knowledge::CapnObject<gams::types::Imu>(imu_b));

   madara::knowledge::CapnObject<gams::types::Imu> imu_ = k.get("imu").to_any<madara::knowledge::CapnObject<gams::types::Imu> >();

   TEST_EQ(imu_.reader().getHeader().getStamp(), 10);
   TEST_EQ(imu_.reader().getHeader().getFrameId().cStr(), std::string("world"));
   TEST_EQ((signed)imu_.reader().getHeader().getSeq(), 100);
   TEST_EQ(imu_.reader().getLinearAccelerationCovariance()[0], 0.01);
   TEST_EQ(imu_.reader().getLinearAccelerationCovariance()[1], 0.03);
}
Example #2
0
/*
  \brief Another sanity and demo test. Shows the user how to interface with a LaserScan type that contains a big array of data, as well as shows that a standard type that is generated is working.
*/
void test_scan()
{
   // Building the object
   ::capnp::MallocMessageBuilder scan_b;
   auto scan = scan_b.initRoot<gams::types::LaserScan>();
   
   // Setting some scan fields
   scan.setAngleMin(0);
   scan.setAngleMax(M_PI);

   // Initializing a scan data list and populating it
   auto ranges = scan.initRanges(1000);
   ranges.set(0, 1.0);
   ranges.set(999, 1.0);

   // Incorrect
   //scan.setRanges(ranges.asReader());

   // Setting to KB
   k.set_any("scan", madara::knowledge::CapnObject<gams::types::LaserScan>(scan_b));

   // Retrieving from KB
   madara::knowledge::CapnObject<gams::types::LaserScan> k_scan = k.get("scan").to_any<madara::knowledge::CapnObject<gams::types::LaserScan> >();

   TEST_EQ(k_scan.reader().getAngleMin(), 0);
   TEST_GT((k_scan.reader().getAngleMax() - M_PI), 0);   
   TEST_EQ(k_scan.reader().getRanges()[0], 1.0);
   TEST_EQ(k_scan.reader().getRanges()[999], 1.0);      
}
Example #3
0
/// Tests logicals operators (&&, ||)
void write_transported_files (
  madara::knowledge::KnowledgeBase & knowledge)
{
  knowledge.write_file ("a_tree", "/files/an_xml_file_transported.xml");
  knowledge.write_file ("double", "/files/twelve_transported.txt");
  knowledge.write_file ("ten", "/files/ten_transported.txt");
  knowledge.write_file ("hello_world", "/files/hello_world_transported.txt");
  knowledge.write_file ("sample", "/files/sample_transported.jpg");
}
Example #4
0
void
test_hz(engine::KnowledgeBase & knowledge,
  controllers::BaseController & loop, double hz = 0.0, double duration = 10.0)
{
  algorithm->reset_counters();
  std::cerr << "Testing " << duration << "s experiment with "
    << hz << "hz.\n";
  loop.run_hz(hz, duration);
  knowledge.set(".loops", algorithm->loops);

#ifndef _MADARA_NO_KARL_
  knowledge.evaluate(".loop_speed = .loops / 10");
  knowledge.evaluate(".legible_speed = to_legible_hertz(.loop_speed)");
#else
  knowledge.set(".loop_speed", algorithm->loops / 10);
#endif
  knowledge.print(
    "  Results: {.loops} loop executions @ {.legible_speed}\n");

  if (hz == 0.0 ||((double)algorithm->loops > hz * .1 * duration) || 
      algorithm->loops > 100)
  {
    knowledge.print(
      "  SUCCESS: {.loops} > hz * .1 * duration\n");
  }
  else
  {
    knowledge.print(
      "  FAIL: {.loops} < hz * .1 * duration\n");
    ++gams_fails;
  }
}
Example #5
0
void
gams::variables::SearchArea::init_vars(
  madara::knowledge::KnowledgeBase & knowledge,
  const std::string & area_name)
{
  // set name
  name = area_name;

  // swarm commands are prefixed with "swarm.movement_command"
  std::string prefix("search_area");
  prefix += ".";
  prefix += area_name;

  madara::knowledge::KnowledgeRecord region_id = knowledge.get(prefix);
  
  // initialize the variable containers
  //region.init_vars(knowledge, region_id.to_string());
}
Example #6
0
    /**
      * Initializes thread with MADARA context
      * @param   context   context for querying current program state
      **/
    virtual void init(madara::knowledge::KnowledgeBase & knowledge)
    {
      data_ = knowledge;
      xyz_velocity_.set_name(".xyz_velocity", data_, 3);

      std::string handle = knowledge.get(".osc.local.handle").to_string();

      madara_logger_ptr_log(gams::loggers::global_logger.get(),
        gams::loggers::LOG_ALWAYS,
        "SenseThread::init: " \
        "%s: attempting to open handle %s\n",
        knowledge.get(".prefix").to_string().c_str(),
        handle.c_str());

      if (handle != "")
      {
        bool result = joystick_.open_handle(handle);

        if (result)
        {
          madara_logger_ptr_log(gams::loggers::global_logger.get(),
            gams::loggers::LOG_ALWAYS,
            "SenseThread::init: " \
            "%s: SUCCESS: mapped to joystick %s\n",
            knowledge.get(".prefix").to_string().c_str(),
            handle.c_str());
        }
        else
        {
          madara_logger_ptr_log(gams::loggers::global_logger.get(),
            gams::loggers::LOG_ALWAYS,
            "SenseThread::init: " \
            "%s: FAIL: cannot map to joystick %s\n",
            knowledge.get(".prefix").to_string().c_str(),
            handle.c_str());
        }
        
      }

      inverted_y_ = knowledge.get(".osc.local.inverted_y").is_true();
      inverted_z_ = knowledge.get(".osc.local.inverted_z").is_true();
      flip_xy_ = knowledge.get(".osc.local.flip_xy").is_true();
    }
Example #7
0
bool
gams::pose::Region::from_container_impl(
  madara::knowledge::KnowledgeBase& kb, const std::string& name)
{
  if (!check_valid_type(kb, name))
  {
    madara_logger_ptr_log(gams::loggers::global_logger.get(),
      gams::loggers::LOG_ERROR,
      "gams::pose::Region::from_container:" \
      " \"%s\" is not a valid Region\n", name.c_str());
    return false;
  }

  madara_logger_ptr_log(gams::loggers::global_logger.get(),
    gams::loggers::LOG_DETAILED,
    "gams::pose::Region::from_container:" \
    " name = %s\n", name.c_str());

  // get type
  madara::knowledge::KnowledgeRecord type = kb.get(name + ".type");
  if (!type.exists())
  {
    madara_logger_ptr_log(gams::loggers::global_logger.get(),
      gams::loggers::LOG_ERROR,
      "gams::pose::Region::from_container:" \
      " \"%s.type\" does not exist in knowledge base\n", name.c_str());
    return false;
  }
  type_ =(unsigned int)type.to_integer();

  // set name if necessary
  if (name_ == "")
    name_ = name;

  // get vertices
  switch(type_)
  {
    case 0: // arbitrary convex polygon
    {
      madara_logger_ptr_log(gams::loggers::global_logger.get(),
        gams::loggers::LOG_DETAILED,
         "gams::pose::Region::from_container:" \
         " type is arbitrary convex polygon\n");

      // get size
      madara::knowledge::KnowledgeRecord num_verts = kb.get(name + ".size");
      if (!num_verts.exists())
      {
        madara_logger_ptr_log(gams::loggers::global_logger.get(),
          gams::loggers::LOG_ERROR,
          "gams::pose::Region::from_container:" \
          " \"%s.size\" does not exist in knowledge base\n", name.c_str());
        return false;
      }
      Integer num = num_verts.to_integer();
      vertices.resize(num);

      madara_logger_ptr_log(gams::loggers::global_logger.get(),
        gams::loggers::LOG_DETAILED,
        "gams::pose::Region::from_container:" \
        " size is %u\n", num);

      // get each of the vertices
      madara::knowledge::containers::Vector vertices_knowledge;
      vertices_knowledge.set_name(name, kb);
      vertices_knowledge.resize();
      for (Integer i = 0; i < num; ++i)
      {
        std::vector<double> coords(vertices_knowledge[i].to_doubles());

        if (coords.size() == 2)
        {
          madara_logger_ptr_log(gams::loggers::global_logger.get(),
            gams::loggers::LOG_DETAILED,
            "gams::pose::Region::from_container:" \
            " Adding coordinate(%f lat, %f lng)\n",
            coords[0], coords[1]);
          vertices[i] = Position(pose::gps_frame(), coords[1], coords[0]);
        }
        else if (coords.size() == 3)
        {
          madara_logger_ptr_log(gams::loggers::global_logger.get(),
            gams::loggers::LOG_DETAILED,
            "gams::pose::Region::from_container:" \
            " Adding coordinate(%f lat, %f lng, %f alt)\n",
            coords[0], coords[1], coords[2]);
          vertices[i] = Position(pose::gps_frame(), coords[1], coords[0], coords[2]);
        }
        else
        {
          madara_logger_ptr_log(gams::loggers::global_logger.get(),
            gams::loggers::LOG_ERROR,
            "gams::pose::Region::from_container:" \
            " ERROR: invalid coordinate type at %s.%u\n", name.c_str(), i);
          return false;
        }
      }

      calculate_bounding_box();

      break;
    }
    default:
    {
      madara_logger_ptr_log(gams::loggers::global_logger.get(),
        gams::loggers::LOG_ERROR,
        "gams::pose::Region::from_container:" \
        " ERROR: invalid region type %" PRId64 ".\n", type_);
      return false;
    }
  }

  return true;
}
Example #8
0
/// Tests logicals operators (&&, ||)
void read_and_create_files (madara::knowledge::KnowledgeBase & knowledge)
{
  ACE_TRACE (ACE_TEXT ("test_assignments"));

  knowledge.clear ();

  knowledge.read_file ("sample", "/files/sample.jpg");
  knowledge.write_file ("sample", "/files/sample_copy.jpg");

  knowledge.set (".a_string", "Hello world!");
  knowledge.write_file (".a_string", "/files/hello_world.txt");
  knowledge.read_file ("hello_world", "/files/hello_world.txt");
  knowledge.write_file ("hello_world", "/files/hello_world_copy.txt");
  
  knowledge.set (".an_integer", madara::knowledge::KnowledgeRecord::Integer (10));
  knowledge.write_file (".an_integer", "/files/ten.txt");
  knowledge.read_file ("ten", "/files/ten.txt");
  knowledge.write_file ("ten", "/files/ten_copy.txt");
  
  knowledge.set (".a_double", 12.5);
  knowledge.write_file (".a_double", "/files/twelve.txt");
  knowledge.read_file ("double", "/files/twelve.txt");
  knowledge.write_file ("double", "/files/twelve_copy.txt");

  knowledge.set (".an_xml",
    "<tree><leaf>15</leaf><leaf>This leaf is empty.</leaf></tree>");
  knowledge.write_file (".an_xml", "/files/an_xml_file.xml");
  knowledge.read_file ("a_tree", "/files/an_xml_file.xml");
  knowledge.write_file ("a_tree", "/files/an_xml_file_copy.xml");

  
  knowledge.set ("finished_transmitting");
  
}
Example #9
0
/// Tests logicals operators (&&, ||)
void create_arrays(madara::knowledge::KnowledgeBase& knowledge)
{
  knowledge.clear();

  std::vector<double> doubles_vector;
  doubles_vector.resize(5);
  doubles_vector[0] = 2.0 / 3;
  doubles_vector[1] = 5.0 / 4;
  doubles_vector[2] = -3.14159;
  doubles_vector[3] = 42.0;
  doubles_vector[4] = 3000.5238;

  knowledge.set(
      "doubles_vector", doubles_vector, madara::knowledge::EvalSettings::SEND);

  madara::knowledge::KnowledgeRecord::Integer* integer_array =
      new madara::knowledge::KnowledgeRecord::Integer[3];
  integer_array[0] = 0;
  integer_array[1] = 1;
  integer_array[2] = 2;

  knowledge.set("integers_vector", integer_array, 3,
      madara::knowledge::EvalSettings::SEND);

#ifndef _MADARA_NO_KARL_
  knowledge.evaluate(
      "var_array[1] = 3.0", madara::knowledge::EvalSettings::SEND);
  knowledge.evaluate(
      "var_array[3] = 20", madara::knowledge::EvalSettings::SEND);
#else
  knowledge.set_index(
      "var_array", 1, 3.0, madara::knowledge::EvalSettings::SEND);
  knowledge.set_index(
      "var_array", 3, Integer(3.0), madara::knowledge::EvalSettings::SEND);
#endif  // _MADARA_NO_KARL_

  knowledge.set(
      "finished_transmitting", 1, madara::knowledge::EvalSettings::SEND);
  knowledge.print("Sending the following data sets to id 1\n");
  knowledge.print("doubles_vector = [{doubles_vector}]\n");
  knowledge.print("integers_vector = [{integers_vector}]\n");
  knowledge.print("var_array = [{var_array}]\n");

  madara::knowledge::KnowledgeRecord::set_precision(10);

  knowledge.print("Setting precision to 10 and reprinting\n");
  knowledge.print("doubles_vector = [{doubles_vector}]\n");
  knowledge.print("integers_vector = [{integers_vector}]\n");
  knowledge.print("var_array = [{var_array}]\n");

  delete[] integer_array;
}
Example #10
0
/// Tests logicals operators (&&, ||)
void write_transported_arrays(madara::knowledge::KnowledgeBase& knowledge)
{
  knowledge.print("\nReceived the following arrays from id 0\n");
  knowledge.print("doubles_vector = [{doubles_vector}]\n");
  knowledge.print("integers_vector = [{integers_vector}]\n");
  knowledge.print("var_array = [{var_array}]\n\n");

  madara::knowledge::KnowledgeRecord doubles_vector =
      knowledge.get("doubles_vector");
  doubles_vector.set_index(5, 127.25);
  doubles_vector.set_index(6, 1.2575);

  std::cerr << "adding two doubles (127.25, 1.2575) to the doubles_vector\n";
  std::cerr << "doubles_vector = [" << doubles_vector.to_string(", ") << "]"
            << std::endl;

  madara::knowledge::KnowledgeRecord integers_vector =
      knowledge.get("integers_vector");

  std::cerr << "\nintegers_vector = [" << integers_vector.to_string(", ") << "]"
            << std::endl;

  integers_vector.set_index(7, madara::knowledge::KnowledgeRecord::Integer(7));

  std::cerr << "adding one integer (7) to the integers_vector\n";

  std::cerr << "integers_vector = [" << integers_vector.to_string(", ") << "]"
            << std::endl;

  std::cerr << "adding one double (6.0) to the integers_vector\n";

  integers_vector.set_index(6, 6.0);

  std::cerr << "integers_vector = [" << integers_vector.to_string(", ")
            << "]\n\n";

  knowledge.print("var_array = [{var_array}]\n");
  std::cerr << "copying elements of var_array to var_array\n";

#ifndef _MADARA_NO_KARL_
  knowledge.evaluate(
      "var_array[0] = var_array[3]", madara::knowledge::EvalSettings::SEND);
  knowledge.evaluate(
      "var_array[2] = var_array[1]", madara::knowledge::EvalSettings::SEND);
  knowledge.evaluate(
      "var_array[8] = 100.012", madara::knowledge::EvalSettings::SEND);
#else
  knowledge.set_index("var_array", 0,
      knowledge.retrieve_index("var_array", 3).to_double(),
      madara::knowledge::EvalSettings::SEND);
  knowledge.set_index("var_array", 2,
      knowledge.retrieve_index("var_array", 1).to_double(),
      madara::knowledge::EvalSettings::SEND);
  knowledge.set_index(
      "var_array", 8, 100.012, madara::knowledge::EvalSettings::SEND);
#endif  // _MADARA_NO_KARL_

  knowledge.print("var_array = [{var_array}]\n\n");
}