/// 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;
                }
            }
        }
    }
}
コード例 #2
0
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;

}