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