/**
* All of the area coverage algorithms just check if they have reached close
* enough to their destination. If so, they generate a new destination.
*/
int
gams::algorithms::area_coverage::BaseAreaCoverage::plan(void)
{
if (platform_ && *platform_->get_platform_status()->movement_available)
{
pose::Position loc = platform_->get_location();
pose::Position next_loc(platform_->get_frame(),
next_position_.longitude(), next_position_.latitude(),
next_position_.altitude());
double dist = loc.distance_to(next_loc);
madara_logger_ptr_log(gams::loggers::global_logger.get(),
gams::loggers::LOG_DETAILED,
"gams::algorithms::area_coverage::BaseAreaCoverage::plan:" \
" distance between points is %f(need %f accuracy)\n",
dist, platform_->get_accuracy());
if (loc.approximately_equal(next_loc, platform_->get_accuracy()))
{
madara_logger_ptr_log(gams::loggers::global_logger.get(),
gams::loggers::LOG_DETAILED,
"gams::algorithms::area_coverage::BaseAreaCoverage::plan:" \
" generating new position\n");
generate_new_position();
}
}
return 0;
}
/// Evaluates the node and its children. This does not prune any of
/// the expression tree, and is much faster than the prune function
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallToHostDirs::evaluate (
const madara::knowledge::KnowledgeUpdateSettings & settings)
{
knowledge::KnowledgeRecord return_value;
if (nodes_.size () == 1)
{
std::string statement (nodes_[0]->evaluate (settings).to_string ());
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"System call to_host_dirs is returning the proper directory structure "
"of %s.\n", statement.c_str ());
return madara::utility::clean_dir_name (statement);
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_EMERGENCY,
"KARL COMPILE ERROR: System call to_host_dirs requires 1 argument,"
"e.g., #to_host_dirs ('files/file.txt') will convert to files\file.txt"
" on Windows\n");
}
return return_value;
}
void madara::threads::Threader::run(
const std::string name, BaseThread* thread, bool paused)
{
if (name != "" && thread != 0)
{
std::unique_ptr<WorkerThread> worker(
new WorkerThread(name, thread, control_, data_));
if (paused)
thread->paused = 1;
if (debug_)
worker->debug_ = 1;
(threads_[name] = std::move(worker))->run();
}
else if (thread != 0 && name == "")
{
delete thread;
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_ERROR,
"Threader::run: named thread has an empty name. Deleting new thread.");
throw exceptions::ThreadException(
"Threader::run: named thread has an empty name. Deleting new thread.");
}
else
{
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_ERROR,
"Threader::run: named thread has an empty name.");
throw exceptions::ThreadException(
"Threader::run: named thread has an empty name.");
}
}
/// Evaluates the node and its children. This does not prune any of
/// the expression tree, and is much faster than the prune function
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallToDoubles::evaluate(
const madara::knowledge::KnowledgeUpdateSettings& settings)
{
if (nodes_.size() > 0)
{
madara_logger_ptr_log(logger_, logger::LOG_MINOR,
"System call to_doubles is converting an argument\n");
return knowledge::KnowledgeRecord(
nodes_[0]->evaluate(settings).to_doubles());
}
else
{
madara_logger_ptr_log(logger_, logger::LOG_ERROR,
"madara::expression::SystemCallToDoubles: "
"KARL RUNTIME ERROR:"
"System call to_doubles requires an argument\n");
throw exceptions::KarlException(
"madara::expression::SystemCallToDoubles: "
"KARL RUNTIME ERROR: "
"System call to_doubles requires an argument\n");
}
return knowledge::KnowledgeRecord();
}
void WorkerThread::run(void)
{
try
{
me_ = std::thread(&WorkerThread::svc, this);
#ifndef _WIN32
try_pthread_setname_np(me_.native_handle(), name_.substr(0, 15).c_str());
#endif
std::ostringstream os;
os << std::this_thread::get_id() << " spawned " << me_.get_id()
<< std::endl;
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_MAJOR,
"WorkerThread::WorkerThread(%s):"
" thread started %s\n",
name_.c_str(), os.str().c_str());
}
catch (const std::exception& e)
{
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_MAJOR,
"WorkerThread::WorkerThread(%s):"
" failed to create thread: %s\n",
name_.c_str(), e.what());
throw;
}
}
/// Evaluates the node and its children. This does not prune any of
/// the expression tree, and is much faster than the prune function
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallFragment::evaluate (
const madara::knowledge::KnowledgeUpdateSettings & settings)
{
knowledge::KnowledgeRecord return_value;
if (nodes_.size () == 3)
{
// grab the first and last ranges
unsigned int first =
(unsigned int) nodes_[1]->evaluate (settings).to_integer ();
unsigned int last =
(unsigned int) nodes_[2]->evaluate (settings).to_integer ();
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"System call fragment is returning segment [%d-%d].\n",
first, last);
return nodes_[0]->evaluate (settings).fragment (first, last);
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_EMERGENCY,
"KARL RUNTIME ERROR: System call fragment"
" requires three arguments, e.g."
" #fragment ('hello world', 0, 4) will return 'hello'\n");
}
return return_value;
}
platforms::BasePlatform *
platforms::PlatformFactoryRepository::create (
const std::string & type,
const madara::knowledge::KnowledgeMap & args)
{
madara_logger_ptr_log (gams::loggers::global_logger.get (),
gams::loggers::LOG_MINOR,
"gams::platforms::PlatformFactoryRepository::create(" \
"%s,...)\n", type.c_str ());
BasePlatform * result (0);
if (type != "")
{
FactoryMap::iterator it = factory_map_.find (type);
if (it != factory_map_.end ())
{
result = it->second->create (args, knowledge_, sensors_, platforms_,
self_);
}
else
{
madara_logger_ptr_log (gams::loggers::global_logger.get (),
gams::loggers::LOG_ALWAYS,
"gams::platforms::PlatformFactoryRepository::create:" \
" could not find \"%s\" platform.\n", type.c_str ());
}
}
return result;
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord
madara::expression::CompositeLessThanEqualNode::prune(bool& can_change)
{
bool left_child_can_change = false;
bool right_child_can_change = false;
madara::knowledge::KnowledgeRecord left_value;
madara::knowledge::KnowledgeRecord right_value;
if (this->left_)
{
left_value = this->left_->prune(left_child_can_change);
if (!left_child_can_change && dynamic_cast<LeafNode*>(left_) == 0)
{
delete this->left_;
this->left_ = new LeafNode(*(this->logger_), left_value);
}
}
else
{
madara_logger_ptr_log(logger_, logger::LOG_ERROR,
"madara::expression::CompositeLessThanEqualNode: "
"KARL COMPILE ERROR: Less-than-equal-to has no left expression\n");
throw exceptions::KarlException(
"madara::expression::CompositeLessThanEqualNode: "
"KARL COMPILE ERROR: "
"Node has no right expression\n");
}
if (this->right_)
{
right_value = this->right_->prune(right_child_can_change);
if (!right_child_can_change && dynamic_cast<LeafNode*>(right_) == 0)
{
delete this->right_;
this->right_ = new LeafNode(*(this->logger_), right_value);
}
}
else
{
madara_logger_ptr_log(logger_, logger::LOG_ERROR,
"madara::expression::CompositeLessThanEqualNode: "
"KARL COMPILE ERROR: Less-than-equal-to has no right expression\n");
throw exceptions::KarlException(
"madara::expression::CompositeLessThanEqualNode: "
"KARL COMPILE ERROR: "
"Node has no right expression\n");
}
can_change = left_child_can_change || right_child_can_change;
return knowledge::KnowledgeRecord(left_value <= right_value);
}
/// Evaluates the node and its children. This does not prune any of
/// the expression tree, and is much faster than the prune function
madara::knowledge::KnowledgeRecord
madara::expression::VariableDecrementNode::evaluate (
const madara::knowledge::KnowledgeUpdateSettings & settings)
{
madara::knowledge::KnowledgeRecord rhs;
if (rhs_)
rhs = rhs_->evaluate (settings);
else
rhs = value_;
if (var_)
{
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"CompositeAssignmentNode::evaluate: "
"Attempting to set variable %s to %s.\n",
var_->expand_key ().c_str (),
rhs.to_string ().c_str ());
knowledge::KnowledgeRecord result (var_->evaluate (settings) - rhs);
var_->set (result, settings);
return result;
}
else if (array_)
{
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"CompositeAssignmentNode::evaluate: "
"Attempting to set index of var %s to %s.\n",
array_->expand_key ().c_str (),
rhs.to_string ().c_str ());
knowledge::KnowledgeRecord result (array_->evaluate (settings) - rhs);
array_->set (result, settings);
return result;
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"CompositeAssignmentNode::evaluate: "
"left hand side was neither a variable nor an array reference. "
"Check your expression for errors.\n",
array_->expand_key ().c_str (),
rhs.to_string ().c_str ());
}
// return the value
return rhs;
}
madara::knowledge::containers::NativeDoubleVector::~NativeDoubleVector ()
{
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::destructor called on %s\n",
this->name_.c_str ());
}
int
madara::knowledge::containers::NativeDoubleVector::set (
size_t index,
type value,
const KnowledgeUpdateSettings & settings)
{
int result = -1;
if (context_)
{
ContextGuard context_guard (*context_);
MADARA_GUARD_TYPE guard (mutex_);
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::set: %s: setting element [%d] "
"to %f with custom settings\n",
this->name_.c_str (),
(int)index, value);
result = context_->set_index (vector_, index, value, settings);
}
return result;
}
int
gams::algorithms::Move::execute (void)
{
int result (OK);
bool is_finished = status_.finished == 1;
if (platform_ && *platform_->get_platform_status ()->movement_available
&& !is_finished)
{
madara_logger_ptr_log (gams::loggers::global_logger.get (),
gams::loggers::LOG_MAJOR,
"Move::execute:" \
" calling platform->move(\"%s\")\n",
locations_[move_index_].to_string ().c_str ());
// allow GPSPosition to do the conversion for lat/lon
utility::GPSPosition next = locations_[move_index_];
platform_->move (next);
}
else if (is_finished)
{
result |= FINISHED;
}
return result;
}
void
gams::groups::GroupTransient::sync(void)
{
madara_logger_ptr_log(gams::loggers::global_logger.get(),
gams::loggers::LOG_MAJOR,
"gams::groups::GroupTransient:sync" \
" syncing with %s source\n", prefix_.c_str());
if (knowledge_ && prefix_ != "")
{
knowledge::ContextGuard guard(*knowledge_);
// sync with map is always expensive. Sync and clear
members_.sync_keys();
fast_members_.clear();
// get the new list of keys
std::vector <std::string> keys;
members_.keys(keys);
// add the keys with values to the AgentMap fast_members_
for (size_t i = 0; i < keys.size(); ++i)
{
const std::string key = keys[i];
if (members_[key].to_integer() != 0)
{
fast_members_[key] = members_[key].to_integer();
}
}
}
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord madara::expression::CompositeNotNode::prune(
bool& can_change)
{
bool right_child_can_change = false;
madara::knowledge::KnowledgeRecord right_value;
if (this->right_)
{
right_value = this->right_->prune(right_child_can_change);
if (!right_child_can_change && dynamic_cast<LeafNode*>(right_) == 0)
{
delete this->right_;
this->right_ = new LeafNode(*(this->logger_), right_value);
}
}
else
{
madara_logger_ptr_log(logger_, logger::LOG_ERROR,
"madara::expression::CompositeNotNode: "
"KARL COMPILE ERROR: "
"Logical-not has no right expression\n");
throw exceptions::KarlException("madara::expression::CompositeNotNode: "
"KARL COMPILE ERROR: "
"Node has no right expression\n");
}
can_change = right_child_can_change;
return knowledge::KnowledgeRecord(!right_value);
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallFragment::prune (bool & can_change)
{
// user can always change a function, and we have no control over
// what it does. Consequently, a function node cannot be pruned out
// under any situation
can_change = true;
madara::knowledge::KnowledgeRecord result;
for (ComponentNodes::iterator i = nodes_.begin (); i != nodes_.end ();
++i)
{
bool arg_can_change = false;
result = (*i)->prune (arg_can_change);
if (!arg_can_change && dynamic_cast <LeafNode *> (*i) == 0)
{
delete *i;
*i = new LeafNode (*(this->logger_), result);
}
}
if (nodes_.size () != 3)
{
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"KARL COMPILE ERROR: System call fragment"
" requires three arguments, e.g."
" #fragment ('hello world', 0, 4) will return 'hello'\n");
}
return result;
}
/// Evaluates the node and its children. This does not prune any of
/// the expression tree, and is much faster than the prune function
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallRandDouble::evaluate (
const madara::knowledge::KnowledgeUpdateSettings & settings)
{
double floor (0.0), ceiling (1.0);
bool update_srand = true;
if (nodes_.size () > 0)
{
floor = nodes_[0]->evaluate (settings).to_double ();
if (nodes_.size () > 1)
{
ceiling = nodes_[1]->evaluate (settings).to_double ();
if (nodes_.size () > 2)
{
update_srand = bool (nodes_[2]->evaluate (settings).to_integer ());
}
}
}
madara_logger_ptr_log (logger_, logger::LOG_MINOR,
"System call rand_double called with %f, %f, %d\n",
floor, ceiling, update_srand);
return utility::rand_double (floor, ceiling, update_srand);
}
void
madara::knowledge::containers::NativeDoubleVector::set_name (
const std::string & var_name,
ThreadSafeContext & knowledge, int size)
{
if (context_ != &knowledge || name_ != var_name)
{
context_ = &knowledge;
ContextGuard context_guard (*context_);
MADARA_GUARD_TYPE guard (mutex_);
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::set_name: setting name to %s\n",
var_name.c_str ());
name_ = var_name;
vector_ = knowledge.get_ref (var_name, settings_);
if (size > 0)
resize (size_t (size));
}
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallRandDouble::prune (bool & can_change)
{
can_change = true;
madara::knowledge::KnowledgeRecord result;
for (ComponentNodes::iterator i = nodes_.begin (); i != nodes_.end ();
++i)
{
bool arg_can_change = false;
result = (*i)->prune (arg_can_change);
if (!arg_can_change && dynamic_cast <LeafNode *> (*i) == 0)
{
delete *i;
*i = new LeafNode (*(this->logger_), result);
}
}
if (nodes_.size () > 3)
{
madara_logger_ptr_log (logger_, logger::LOG_EMERGENCY,
"KARL COMPILE ERROR: System call rand_double"
" can have up to three arguments, 1) floor, "
"2) ceiling and 3) whether to set the random seed\n");
}
return result;
}
gams::algorithms::BaseAlgorithm *
gams::algorithms::NullAlgorithmFactory::create (
const madara::knowledge::KnowledgeMap & /*args*/,
madara::knowledge::KnowledgeBase * knowledge,
platforms::BasePlatform * platform,
variables::Sensors * sensors,
variables::Self * self,
variables::Agents * /*agents*/)
{
BaseAlgorithm * result (0);
if (knowledge && sensors && platform && self)
{
result = new NullAlgorithm (knowledge, platform, sensors, self);
}
if (result == 0)
{
madara_logger_ptr_log (gams::loggers::global_logger.get (),
gams::loggers::LOG_MAJOR,
"gams::algorithms::NullAlgorithmFactory::create:" \
" unknown error creating Null algorithm\n");
}
return result;
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord
madara::expression::CompositePreincrementNode::prune (bool & can_change)
{
bool right_child_can_change = false;
madara::knowledge::KnowledgeRecord right_value;
if (this->right_)
{
right_value = this->right_->prune (right_child_can_change);
if (!right_child_can_change && dynamic_cast <LeafNode *> (right_) == 0)
{
delete this->right_;
this->right_ = new LeafNode (*(this->logger_), right_value);
}
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_EMERGENCY,
"KARL COMPILE ERROR: Preincrement has no right expression\n");
exit (-1);
}
can_change = right_child_can_change;
return ++right_value;
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord
madara::expression::SystemCallToHostDirs::prune (bool & can_change)
{
// user can always change a function, and we have no control over
// what it does. Consequently, a function node cannot be pruned out
// under any situation
can_change = true;
madara::knowledge::KnowledgeRecord result;
if (nodes_.size () == 1)
{
bool arg_can_change = false;
result = nodes_[0]->prune (arg_can_change);
if (!arg_can_change && dynamic_cast <LeafNode *> (nodes_[0]) == 0)
{
delete nodes_[0];
nodes_[0] = new LeafNode (*(this->logger_), result);
}
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_EMERGENCY,
"KARL COMPILE ERROR: System call to_host_dirs requires 1 argument,"
"e.g., #to_host_dirs ('files/file.txt') will convert to files\file.txt"
" on Windows\n");
}
return result;
}
madara::knowledge::containers::NativeDoubleVectorStaged::
NativeDoubleVectorStaged(const KnowledgeUpdateSettings& settings)
: BaseContainer("", settings), context_(0), has_changed_(false)
{
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_MAJOR,
"NativeDoubleVectorStaged::constructor: new object\n");
}
int
gams::platforms::OscJoystickPlatform::orient(const pose::Orientation & target,
const pose::OrientationBounds &bounds)
{
// update variables
BasePlatform::orient(target, bounds);
madara_logger_ptr_log(gams::loggers::global_logger.get(),
gams::loggers::LOG_TRACE,
"gams::platforms::OscJoystickPlatform::orient:" \
" %s: requested target \"%f,%f,%f\"\n",
self_->agent.prefix.c_str(), target.rx(), target.ry(), target.rz());
// convert from input reference frame to vrep reference frame, if necessary
pose::Orientation new_target(get_frame(), target);
last_orient_ = new_target;
utility::OscUdp::OscMap values;
std::vector<double> yaw_velocity;
yaw_velocity.push_back(0);
values[yaw_velocity_prefix_] = yaw_velocity;
osc_.send(values);
// we're not changing orientation. this has to be done for move alg to work
return PLATFORM_ARRIVED;
}
void
madara::knowledge::containers::NativeDoubleVector::transfer_to (
NativeDoubleVector & other)
{
if (context_ && other.context_)
{
ContextGuard context_guard (*context_);
ContextGuard other_context_guard (*other.context_);
MADARA_GUARD_TYPE guard (mutex_);
MADARA_GUARD_TYPE guard2 (other.mutex_);
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::transfer_to: %s transfering to %s\n",
this->name_.c_str (),
other.name_.c_str ());
size_t other_size = other.size ();
size_t this_size = this->size ();
if (this_size > 0)
{
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MINOR,
"NativeDoubleVector::transfer_to: %s has elements\n",
this->name_.c_str ());
size_t size = other_size + this_size;
other.resize ((int)size);
knowledge::KnowledgeRecord rhs (other.context_->get (other.vector_));
knowledge::KnowledgeRecord lhs (context_->get (vector_));
rhs.set_index (size - 1, lhs.retrieve_index (this_size - 1).to_double ());
for (size_t i = 0, j = other_size; i < this_size - 1; ++i, ++j)
{
rhs.set_index (other_size, lhs.retrieve_index (i).to_double ());
}
other.context_->set (other.vector_, rhs, other.settings_);
this->resize (0);
}
}
}
madara::knowledge::containers::BaseContainer*
madara::knowledge::containers::NativeDoubleVectorStaged::clone(void) const
{
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_MAJOR,
"NativeDoubleVectorStaged::clone: cloning %s\n", this->name_.c_str());
return new NativeDoubleVectorStaged(*this);
}
madara::knowledge::containers::NativeDoubleVectorStaged::
NativeDoubleVectorStaged(const NativeDoubleVectorStaged& rhs)
: BaseContainer(rhs), context_(rhs.context_), vector_(rhs.vector_)
{
madara_logger_ptr_log(logger::global_logger.get(), logger::LOG_MAJOR,
"NativeDoubleVectorStaged::copy constructor called on %s\n",
rhs.name_.c_str());
}
/// Prune the tree of unnecessary nodes.
/// Returns evaluation of the node and sets can_change appropriately.
/// if this node can be changed, that means it shouldn't be pruned.
madara::knowledge::KnowledgeRecord
madara::expression::CompositeGreaterThanEqualNode::prune (bool & can_change)
{
bool left_child_can_change = false;
bool right_child_can_change = false;
madara::knowledge::KnowledgeRecord left_value;
madara::knowledge::KnowledgeRecord right_value;
if (this->left_)
{
left_value = this->left_->prune (left_child_can_change);
if (!left_child_can_change && dynamic_cast <LeafNode *> (left_) == 0)
{
delete this->left_;
this->left_ = new LeafNode (*(this->logger_), left_value);
}
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_DETAILED,
"KARL COMPILE ERROR: Greater-than-equal-to has no left expression\n");
exit (-1);
}
if (this->right_)
{
right_value = this->right_->prune (right_child_can_change);
if (!right_child_can_change && dynamic_cast <LeafNode *> (right_) == 0)
{
delete this->right_;
this->right_ = new LeafNode (*(this->logger_), right_value);
}
}
else
{
madara_logger_ptr_log (logger_, logger::LOG_DETAILED,
"KARL COMPILE ERROR: Greater-than-equal-to has no right expression\n");
exit (-1);
}
can_change = left_child_can_change || right_child_can_change;
return knowledge::KnowledgeRecord::Integer (left_value >= right_value);
}
void
madara::knowledge::containers::NativeDoubleVector::modify (void)
{
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::modify called\n");
ContextGuard context_guard (*context_);
if (context_ && name_ != "")
{
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::modify: context is valid. Marking %s.\n",
this->name_.c_str ());
context_->mark_modified (vector_);
}
}
/**
* 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();
}
madara::knowledge::KnowledgeRecord
madara::knowledge::containers::NativeDoubleVector::to_record (
void) const
{
madara_logger_ptr_log (logger::global_logger.get (),
logger::LOG_MAJOR,
"NativeDoubleVector::to_record: %s: retrieving record\n",
this->name_.c_str ());
return context_->get (this->vector_, settings_);
}
