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