/* \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); }
/* \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); }
/// 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"); }
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; } }
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()); }
/** * 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(); }
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; }
/// 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"); }
/// 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; }
/// 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"); }