/// Callback for the receiving loop when receiving valid json
///
void SbrioDriver::_update_state(JSONNode& node, void* id)
{
SbrioDriver * driver_pointer = static_cast<SbrioDriver*>(id);
// We got a node, get the name of the root node
JSONNode::const_iterator root_iter = node.begin();
// Maybe there is no root.
if (root_iter == node.end())
return;
std::string root_name = root_iter->name();
boost::regex expression("(.*)_names");
boost::smatch regex_result;
// Handle initialization messages.
if(boost::regex_match(root_name, regex_result, expression))
{
driver_pointer->_root_header = regex_result[1];
// If the sbRIO has already reported its names, we wil disregard it.
if (! driver_pointer->initialized())
{
JSONNode::const_iterator name_type = root_iter->begin();
for(; name_type != root_iter->end(); ++name_type)
{
// Get the position names of the joint (and initialize its velocity and effort).
if (name_type->name() == "position")
{
boost::lock_guard<boost::mutex> state_guard(driver_pointer->state_mutex);
for(JSONNode::const_iterator name_array = name_type->begin(); name_array != name_type->end(); ++name_array)
{
std::string joint_name = name_array->as_string();
driver_pointer->joint_names.push_back(joint_name);
driver_pointer->joint_positions.push_back(0.0);
driver_pointer->joint_velocities.push_back(0.0);
driver_pointer->joint_efforts.push_back(0.0);
driver_pointer->joint_setpoints.push_back(0.0);
driver_pointer->joint_setpoints_mask.push_back(false);
}
}
// Get the names of the joints that take setpoints, and enable them in the mask.
if (name_type->name() == "setpoints")
{
boost::lock_guard<boost::mutex> state_guard(driver_pointer->state_mutex);
for(JSONNode::const_iterator name_array = name_type->begin(); name_array != name_type->end(); ++name_array)
{
std::string joint_name = name_array->as_string();
for(size_t i = 0; i < driver_pointer->joint_names.size(); ++i)
{
if (joint_name == driver_pointer->joint_names[i])
{
driver_pointer->joint_setpoints_mask[i] = true;
break;
}
}
}
}
// Get the names of the motors, for active state and enabling.
if (name_type->name() == "motoractive")
{
boost::lock_guard<boost::mutex> state_guard(driver_pointer->state_mutex);
for(JSONNode::const_iterator name_array = name_type->begin(); name_array != name_type->end(); ++name_array)
{
std::string motor_name = name_array->as_string();
driver_pointer->motor_names.push_back(motor_name);
driver_pointer->motors_active.push_back(false);
driver_pointer->motors_enabled.push_back(false);
}
}
}
driver_pointer->_initialized = true;
}
}
// Handle state updates.
else
{
boost::lock_guard<boost::mutex> state_guard(driver_pointer->state_mutex);
JSONNode::const_iterator name_type = root_iter->begin();
for(; name_type != root_iter->end(); ++name_type)
{
std::string type = name_type->name();
if (type == "position")
{
size_t i = 0;
for(JSONNode::const_iterator pos = name_type->begin(); pos != name_type->end(); ++pos)
{
double joint_pos = pos->as_float();
driver_pointer->joint_positions[i++] = joint_pos;
}
}
else if (type == "velocity")
{
size_t i = 0;
for(JSONNode::const_iterator vel = name_type->begin(); vel != name_type->end(); ++vel)
{
double joint_vel = vel->as_float();
driver_pointer->joint_velocities[i++] = joint_vel;
}
}
else if (type == "effort")
{
size_t i = 0;
for(JSONNode::const_iterator eff = name_type->begin(); eff != name_type->end(); ++eff)
{
double joint_eff = eff->as_float();
driver_pointer->joint_efforts[i++] = joint_eff;
}
}
else if (type == "motoractive")
{
size_t i = 0;
for(JSONNode::const_iterator act = name_type->begin(); act != name_type->end(); ++act)
{
bool motor_act = act->as_bool();
driver_pointer->motors_active[i++] = motor_act;
}
}
}
}
}
Handle<Object> AtJsonParser::parse( const JSONNode &n ) {
Isolate* isolate = Isolate::GetCurrent();
Handle<Object> ret = Object::New( isolate );
JSONNode::const_iterator i = n.begin();
while ( i != n.end() ) {
std::string node_name = i->name();
if ( node_name == "serverTime" ||
node_name == "response" ||
node_name == "data" ||
node_name == "tick" ||
node_name == "ATBarHistory" ||
node_name == "ATSymbol" ||
node_name == "ATLoginResponse" ||
node_name == "time" ||
node_name == "open" ||
node_name == "high" ||
node_name == "low" ||
node_name == "close" ||
node_name == "ATTickHistory" ||
node_name == "offsetDatabaseDate" ||
node_name == "tradeLastPrice" ||
node_name == "quoteBidPrice" ||
node_name == "quoteAskPrice" ||
node_name == "ATMarketMoversDb" ||
node_name == "recordSymbol" ||
node_name == "itemSymbol" ||
node_name == "itemLastPrice" ||
node_name == "ATQuoteDb" ||
node_name == "priceData" ||
node_name == "dateTime" ||
node_name == "ATQuoteStream" ||
node_name == "lastPrice" ||
node_name == "bidPrice" ||
node_name == "askPrice" ||
node_name == "prevClose" ||
node_name == "afterMarketClose" ||
node_name == "lastUpdateTime" ||
node_name == "marketMovers" ||
node_name == "closePrice" ||
node_name == "lastDateTime" ||
node_name == "beginDateTime" ||
node_name == "endDateTime"
) {
ret->Set( String::NewFromUtf8( isolate, node_name.c_str() ),
parse( *i ) );
}
else if ( filterAsString( node_name ) ) {
ret->Set( String::NewFromUtf8( isolate, node_name.c_str() ),
String::NewFromUtf8( isolate, i->as_string().c_str() ) );
}
else if ( filterAsNumber( node_name ) ) {
ret->Set( String::NewFromUtf8( isolate, node_name.c_str() ),
Number::New( isolate, i->as_float() ) );
}
else if ( filterAsInteger( node_name ) ) {
ret->Set( String::NewFromUtf8( isolate, node_name.c_str() ),
Integer::New( isolate, i->as_int() ) );
}
else if ( filterAsBoolean( node_name ) ) {
ret->Set( String::NewFromUtf8( isolate, node_name.c_str() ),
Boolean::New( isolate, i->as_bool() ) );
}
else if ( i->type() == JSON_ARRAY ) {
JSONNode::const_iterator j = i->begin();
std::vector< Handle<Object> > vec;
while ( j != i->end() ) {
vec.push_back( parse( *j ) );
++j;
}
Handle<Array> arr = Array::New( isolate, vec.size() );
for ( size_t idx = 0; idx < vec.size(); idx++ ) {
arr->Set( idx, vec[idx] );
}
ret->Set( String::NewFromUtf8( isolate, node_name.c_str() ), arr );
}
++i;
}
return ret;
}