<commit_before>#include <ros.h>
#include <string.h>

#include "MS5837.h"
#include "Adafruit_BNO055.h"
#include "Adafruit_Sensor.h"
#include "DallasTemperature.h"
#include <Wire.h>

#include "std_msgs/String.h"
#include <geometry_msgs/Vector3.h>
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "sensor_msgs/Temperature.h"
#include "sensor_msgs/FluidPressure.h"

#define STANDARD_GRAVITY 9.08665      // [m/s^2]
#define RAD_PER_DEG 0.01745329252     // [1/deg]
#define PASCAL_PER_MILLIBAR 0.01      // [Pa/mbar]
#define MICROTESLA_PER_TESLA 0.000001 // [uT/T]

int id = -1;

ros::NodeHandle nh;

sensor_msgs::Imu imu_raw_msg;
sensor_msgs::MagneticField compass_msg;

sensor_msgs::Temperature imu_temperature_msg;
sensor_msgs::Temperature sensor_temperature_msg;
sensor_msgs::FluidPressure pressure_msg;

std_msgs::String calibration;

ros::Publisher pub_imu("imu/data", &imu_raw_msg);
ros::Publisher pub_mag("imu/mag", &compass_msg);

ros::Publisher pub_pressure("imu/pressure", &pressure_msg);
ros::Publisher pub_imu_temperature("imu/temperature", &imu_temperature_msg);
ros::Publisher pub_sensor_temperature("sensor/temperature", &sensor_temperature_msg);

ros::Publisher pub_calibration("imu/calibration", &calibration);

const int SensorReadDelay = 40;
unsigned long PrevoiusSensorReadMillis = 0;

int dbg_count = 0;

MS5837 barometer;
Adafruit_BNO055 bno055 = Adafruit_BNO055(55);

#define ONE_WIRE_BUS 2
OneWire oneWire(ONE_WIRE_BUS);

DallasTemperature sensors(&oneWire);

// double GyroLsbSens, AccelLsbSens;
void setupIMU();

void setup() {

    //start ROS-node
    nh.initNode();

    // nh.advertise(pub_dbg);
    nh.advertise(pub_imu_temperature);
    nh.advertise(pub_sensor_temperature);

    nh.advertise(pub_pressure);
    nh.advertise(pub_imu);
    nh.advertise(pub_mag);
    nh.advertise(pub_calibration);

    // Initialize the 'Wire' class for communicating over i2c
    Wire.begin();
    barometer.init();
    barometer.read();

    setupIMU();
    // nh.advertise(pub_mag);
}

void setupIMU() {

    /*
        Sets opmode to NDOF by default

        From Adafruit_BNO055.h:

        OPERATION_MODE_CONFIG                                   = 0X00,
        OPERATION_MODE_ACCONLY                                  = 0X01,
        OPERATION_MODE_MAGONLY                                  = 0X02,
        OPERATION_MODE_GYRONLY                                  = 0X03,
        OPERATION_MODE_ACCMAG                                   = 0X04,
        OPERATION_MODE_ACCGYRO                                  = 0X05,
        OPERATION_MODE_MAGGYRO                                  = 0X06,
        OPERATION_MODE_AMG                                      = 0X07,
        OPERATION_MODE_IMUPLUS                                  = 0X08,
        OPERATION_MODE_COMPASS                                  = 0X09,
        OPERATION_MODE_M4G                                      = 0X0A,
        OPERATION_MODE_NDOF_FMC_OFF                             = 0X0B,
        OPERATION_MODE_NDOF                                     = 0X0C

                    ACCEL   MAG     GYRO    RELATIVE        ABSOLUTE
                                            ORIENTATION     ORIENTATION
        ---------------------------------------------------------------
        IMU     |    X    |   -    |   X  |       X        |     -
        COMPASS |    X    |   X    |   -  |       -        |     X
        M4G     |    X    |   X    |   -  |       X        |     -
        NDOF    |    X    |   X    |   X  |       -        |     X
    */

    id = bno055.begin(bno055.OPERATION_MODE_IMUPLUS);

}

void getPressure() {

    // float pressure = barometer.pressure();
    float pressure = barometer.pressure();
    pressure_msg.fluid_pressure = pressure * 100;
    pub_pressure.publish(&pressure_msg);

}

#define BARO 10
int baro_counter = 0;
void getBarometerTemp() {

    if(baro_counter < BARO){
        baro_counter++;
        return;
    }
    else{
        baro_counter = 0;
    }

    float temp = barometer.temperature();
    imu_temperature_msg.temperature = temp;

    pub_imu_temperature.publish(&imu_temperature_msg);

}

#define DALL 10
int dallas_counter = 50;
void getDallasTemp() {

    if(dallas_counter < DALL){
        dallas_counter++;
        return;
    }
    else{
        dallas_counter = 0;
    }

    sensors.requestTemperatures();
    float temp = sensors.getTempCByIndex(0);
    sensor_temperature_msg.temperature = temp;

    pub_sensor_temperature.publish(&sensor_temperature_msg);

}

void getRawIMU() {

    // sensors_event_t event;
    // bno055.getEvent(&event);

    imu::Vector<3> gyro = bno055.getVector(bno055.VECTOR_GYROSCOPE);
    imu_raw_msg.linear_acceleration.x = gyro[0];
    imu_raw_msg.linear_acceleration.y = gyro[1];
    imu_raw_msg.linear_acceleration.z = gyro[2];

    imu::Vector<3> lin_acc = bno055.getVector(bno055.VECTOR_LINEARACCEL);
    imu_raw_msg.angular_velocity.x = lin_acc[0];
    imu_raw_msg.angular_velocity.y = lin_acc[1];
    imu_raw_msg.angular_velocity.z = lin_acc[2];

    imu::Quaternion qs = bno055.getQuat();
    imu_raw_msg.orientation.x = qs.x();
    imu_raw_msg.orientation.y = qs.y();
    imu_raw_msg.orientation.z = qs.z();
    imu_raw_msg.orientation.w = qs.w();

    imu_raw_msg.header.stamp = ros::Time(millis()/1000.0, 0);
    pub_imu.publish(&imu_raw_msg);

}

bool calibrated = false;

void getCalibrationStatus(){

    // lol
    uint8_t sys, gyro, accel, mag;
    sys = gyro = accel = mag = 0;
    bno055.getCalibration(&sys, &gyro, &accel, &mag);
    char buffer[128] = { '\0' };

    if (sys == 3 && gyro == 3 && accel == 3 && mag == 3){
        if(!calibrated){
            sprintf(buffer, "\n All systems go\n");
            calibrated = true;
        }
        return;
    }

    calibrated = false;
    sprintf(buffer, "\nSys: %d, Gyro: %d, Acc: %d, mag: %d\n", sys, gyro, accel, mag);
    String s = String(buffer);
    calibration.data = s.c_str();
    pub_calibration.publish(&calibration);

}

void loop(){

    nh.spinOnce();

    if( millis() - PrevoiusSensorReadMillis >= SensorReadDelay ) {

        PrevoiusSensorReadMillis = millis();

        barometer.read();
        nh.spinOnce();

        getPressure();
        nh.spinOnce();

        getBarometerTemp();
        nh.spinOnce();

        getDallasTemp();
        nh.spinOnce();

        getRawIMU();
        nh.spinOnce();

        getCalibrationStatus();
        nh.spinOnce();

    }
}

<commit_msg>remax imu axes<commit_after>#include <ros.h>
#include <string.h>

#include "MS5837.h"
#include "Adafruit_BNO055.h"
#include "Adafruit_Sensor.h"
#include "DallasTemperature.h"
#include <Wire.h>

#include "std_msgs/String.h"
#include <geometry_msgs/Vector3.h>
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "sensor_msgs/Temperature.h"
#include "sensor_msgs/FluidPressure.h"

#define STANDARD_GRAVITY 9.08665      // [m/s^2]
#define RAD_PER_DEG 0.01745329252     // [1/deg]
#define PASCAL_PER_MILLIBAR 0.01      // [Pa/mbar]
#define MICROTESLA_PER_TESLA 0.000001 // [uT/T]

int id = -1;

ros::NodeHandle nh;

sensor_msgs::Imu imu_raw_msg;
sensor_msgs::MagneticField compass_msg;

sensor_msgs::Temperature imu_temperature_msg;
sensor_msgs::Temperature sensor_temperature_msg;
sensor_msgs::FluidPressure pressure_msg;

std_msgs::String calibration;

ros::Publisher pub_imu("imu/data", &imu_raw_msg);
ros::Publisher pub_mag("imu/mag", &compass_msg);

ros::Publisher pub_pressure("imu/pressure", &pressure_msg);
ros::Publisher pub_imu_temperature("imu/temperature", &imu_temperature_msg);
ros::Publisher pub_sensor_temperature("sensor/temperature", &sensor_temperature_msg);

ros::Publisher pub_calibration("imu/calibration", &calibration);

const int SensorReadDelay = 40;
unsigned long PrevoiusSensorReadMillis = 0;

int dbg_count = 0;

MS5837 barometer;
Adafruit_BNO055 bno055 = Adafruit_BNO055(55);

#define ONE_WIRE_BUS 2
OneWire oneWire(ONE_WIRE_BUS);

DallasTemperature sensors(&oneWire);

// double GyroLsbSens, AccelLsbSens;
void setupIMU();

void setup() {

    //start ROS-node
    nh.initNode();

    // nh.advertise(pub_dbg);
    nh.advertise(pub_imu_temperature);
    nh.advertise(pub_sensor_temperature);

    nh.advertise(pub_pressure);
    nh.advertise(pub_imu);
    nh.advertise(pub_mag);
    nh.advertise(pub_calibration);

    // Initialize the 'Wire' class for communicating over i2c
    Wire.begin();
    barometer.init();
    barometer.read();

    setupIMU();
    // nh.advertise(pub_mag);
}

void setupIMU() {

    /*
        Sets opmode to NDOF by default

        From Adafruit_BNO055.h:

        OPERATION_MODE_CONFIG                                   = 0X00,
        OPERATION_MODE_ACCONLY                                  = 0X01,
        OPERATION_MODE_MAGONLY                                  = 0X02,
        OPERATION_MODE_GYRONLY                                  = 0X03,
        OPERATION_MODE_ACCMAG                                   = 0X04,
        OPERATION_MODE_ACCGYRO                                  = 0X05,
        OPERATION_MODE_MAGGYRO                                  = 0X06,
        OPERATION_MODE_AMG                                      = 0X07,
        OPERATION_MODE_IMUPLUS                                  = 0X08,
        OPERATION_MODE_COMPASS                                  = 0X09,
        OPERATION_MODE_M4G                                      = 0X0A,
        OPERATION_MODE_NDOF_FMC_OFF                             = 0X0B,
        OPERATION_MODE_NDOF                                     = 0X0C

                    ACCEL   MAG     GYRO    RELATIVE        ABSOLUTE
                                            ORIENTATION     ORIENTATION
        ---------------------------------------------------------------
        IMU     |    X    |   -    |   X  |       X        |     -
        COMPASS |    X    |   X    |   -  |       -        |     X
        M4G     |    X    |   X    |   -  |       X        |     -
        NDOF    |    X    |   X    |   X  |       -        |     X
    */

    id = bno055.begin(bno055.OPERATION_MODE_IMUPLUS);
    bno055.setAxisSign(bno055.REMAP_SIGN_P4);
}

void getPressure() {

    // float pressure = barometer.pressure();
    float pressure = barometer.pressure();
    pressure_msg.fluid_pressure = pressure * 100;
    pub_pressure.publish(&pressure_msg);

}

#define BARO 10
int baro_counter = 0;
void getBarometerTemp() {

    if(baro_counter < BARO){
        baro_counter++;
        return;
    }
    else{
        baro_counter = 0;
    }

    float temp = barometer.temperature();
    imu_temperature_msg.temperature = temp;

    pub_imu_temperature.publish(&imu_temperature_msg);

}

#define DALL 10
int dallas_counter = 50;
void getDallasTemp() {

    if(dallas_counter < DALL){
        dallas_counter++;
        return;
    }
    else{
        dallas_counter = 0;
    }

    sensors.requestTemperatures();
    float temp = sensors.getTempCByIndex(0);
    sensor_temperature_msg.temperature = temp;

    pub_sensor_temperature.publish(&sensor_temperature_msg);

}

void getRawIMU() {

    // sensors_event_t event;
    // bno055.getEvent(&event);

    imu::Vector<3> gyro = bno055.getVector(bno055.VECTOR_GYROSCOPE);
    imu_raw_msg.linear_acceleration.x = gyro[0];
    imu_raw_msg.linear_acceleration.y = gyro[1];
    imu_raw_msg.linear_acceleration.z = gyro[2];

    imu::Vector<3> lin_acc = bno055.getVector(bno055.VECTOR_LINEARACCEL);
    imu_raw_msg.angular_velocity.x = lin_acc[0];
    imu_raw_msg.angular_velocity.y = lin_acc[1];
    imu_raw_msg.angular_velocity.z = lin_acc[2];

    imu::Quaternion qs = bno055.getQuat();
    imu_raw_msg.orientation.x = qs.x();
    imu_raw_msg.orientation.y = qs.y();
    imu_raw_msg.orientation.z = qs.z();
    imu_raw_msg.orientation.w = qs.w();

    imu_raw_msg.header.stamp = ros::Time(millis()/1000.0, 0);
    pub_imu.publish(&imu_raw_msg);

}

bool calibrated = false;

void getCalibrationStatus(){

    // lol
    uint8_t sys, gyro, accel, mag;
    sys = gyro = accel = mag = 0;
    bno055.getCalibration(&sys, &gyro, &accel, &mag);
    char buffer[128] = { '\0' };

    if (sys == 3 && gyro == 3 && accel == 3 && mag == 3){
        if(!calibrated){
            sprintf(buffer, "\n All systems go\n");
            calibrated = true;
        }
        return;
    }

    calibrated = false;
    sprintf(buffer, "\nSys: %d, Gyro: %d, Acc: %d, mag: %d\n", sys, gyro, accel, mag);
    String s = String(buffer);
    calibration.data = s.c_str();
    pub_calibration.publish(&calibration);

}

void loop(){

    nh.spinOnce();

    if( millis() - PrevoiusSensorReadMillis >= SensorReadDelay ) {

        PrevoiusSensorReadMillis = millis();

        barometer.read();
        nh.spinOnce();

        getPressure();
        nh.spinOnce();

        getBarometerTemp();
        nh.spinOnce();

        getDallasTemp();
        nh.spinOnce();

        getRawIMU();
        nh.spinOnce();

        getCalibrationStatus();
        nh.spinOnce();

    }
}

<|endoftext|>


<commit_before>//
//  yssa_liveness.cpp
//
//  Created by Edmund Kapusniak on 29/03/2015.
//  Copyright (c) 2015 Edmund Kapusniak. All rights reserved.
//

#include "yssa_liveness.h"
#include "yssa.h"

/*

    Useful papers about liveness analysis:

    http://www.christianwimmer.at/Publications/Wimmer10a/Wimmer10a.pdf

    Linear Scan Register Allocation on SSA Form
        by Christian Wimmer, Michael Franz

    http://hal.archives-ouvertes.fr/docs/00/55/85/09/PDF/RR-7503.pdf

    Computing Liveness Sets for SSA-Form Programs (2011)
        by Florian Brandner, Benoit Boissinot, Alain Darte,
            Benoît Dupont De Dinechin, Fabrice Rastello

*/

/*
    We work on linearized SSA with analysis information.  Live ranges are
    represented as ranges of op indexes in the linearized SSA.

    Live ranges for each op and each variable are calculated.

     -  Our modified SSA form mandates that the live ranges of the definitions
            of a variable do not overlap.
     -  Live ranges can have holes, where ops are not live in certain blocks.
     -  Variables references from exception handlers are live throughout all
            blocks protected by that handler.

*/

typedef std::unordered_map< yssa_opinst*, size_t > yssa_live;

static void successor( yssa_function* function,
                yssa_live* live, yssa_block* prev, yssa_block* block );
static void live_loop( yssa_module* module,
                yssa_loop* loop, const yssa_live& live );

static void add_live_range( yssa_module* module,
                yssa_opinst* op, size_t start, size_t final );
static void union_live_range( yssa_module* module,
                yssa_live_range** pnext, size_t start, size_t final );

static yssa_opinst* YSSA_NOARG = (yssa_opinst*)-1;

void yssa_liveness( yssa_module* module, yssa_function* function )
{
    // Map opinsts to the single call op they are an argument for, or NOARG.

    std::unordered_map< yssa_opinst*, yssa_opinst* > argof;

    // Calculate live ranges for each op.

    for ( int i = (int)function->blocks.size() - 1; i >= 0; --i )
    {
        yssa_block* block = function->blocks.at( i ).get();

        // The end of the live range for each op.
        std::unordered_map< yssa_opinst*, size_t > live;

        // At the bottom of the block, all variables that are live on
        // entry to successor blocks are live.  This includes all
        // references in phi-functions from this code path.
        if ( block->test )
        {
            successor( function, &live, block, block->next );
            successor( function, &live, block, block->fail );

            // The condition is also live at the bottom of the block.
            live.emplace( block->test, block->lfinal );
        }
        else
        {
            successor( function, &live, block, block->next );
        }

        // Visit each op in the block, building liveness information.
        for ( size_t i = block->lfinal; i > block->lstart; --i )
        {
            size_t index = i - 1;
            yssa_opinst* op = function->ops.at( index );

            // If this op is live, then this is the start of its live range.
            auto j = live.find( op );
            if ( j != live.end() )
            {
                // Add range.
                add_live_range( module, op, index, j->second );

                // Op is no longer live.
                live.erase( j );
            }

            // Phi function operands do not create liveness here, but
            // rather at the end of predecessor blocks.
            if ( op->opcode == YSSA_PHI || op->opcode == YSSA_REF )
            {
                continue;
            }

            // All operands which are not already live are live now.
            for ( size_t i = 0; i < op->operand_count; ++i )
            {
                if ( ! op->operand[ i ] )
                    continue;

                live.emplace( op->operand[ i ], index );

                // Update argof.
                if ( op->is_call() )
                {
                    yssa_opinst*& call = argof[ op->operand[ i ] ];
                    if ( ! call || call == op )
                        call = op;
                    else
                        call = YSSA_NOARG;
                }
            }

            if ( op->has_multival() && op->multival )
            {
                live.emplace( op->multival, index );

                // Update argof.
                yssa_opinst* call = argof[ op->multival ];
                if ( ! call || call == op )
                    call = op;
                else
                    call = YSSA_NOARG;
            }
        }

        // This is the live in set of the block.  All live ranges
        // end at the top of the block.
        for ( auto liveop : live )
        {
            yssa_opinst* op = liveop.first;
            add_live_range( module, op, block->lstart, liveop.second );
            block->livein.insert( op );
        }

        // If this block is a loop header, then all ops live at the start
        // of the loop are live throughout the entire loop.
        if ( block->loop && block->loop->header == block )
        {
            live_loop( module, block->loop, live );

            // And none of them are treated as arguments.
            for ( auto liveop : live )
            {
                argof[ liveop.first ] = YSSA_NOARG;
            }
        }

    }

    // Update argof map.
    for ( auto argop : argof )
    {
        if ( argop.second != YSSA_NOARG )
        {
            function->argof.insert( argop );
        }
    }

    // Live range for a variable is the union of live ranges of its
    // definitions (which must not overlap), plus the range of all
    // protected blocks if the variable is used in an xchandler.

    std::unordered_map< yssa_variable*, yssa_opinst* > vargof;

    for ( size_t i = 0; i < function->ops.size(); ++i )
    {
        yssa_opinst* op = function->ops.at( i );
        if ( op->live && ! op->has_associated() && op->variable )
        {
            // Merge live range into live range of variable.
            for ( yssa_live_range* live = op->live; live; live = live->next )
            {
                yssa_live_range** pnext = &op->variable->live;
                union_live_range( module, pnext, live->start, live->final );
            }

            // Update argument status.
            yssa_opinst*& call = vargof[ op->variable ];
            auto j = function->argof.find( op );
            if ( j != function->argof.end() )
            {
                if ( ! call || call == j->second )
                    call = j->second;
                else
                    call = YSSA_NOARG;
            }
            else
            {
                call = YSSA_NOARG;
            }
        }
    }

    for ( size_t i = 0; i < function->blocks.size(); ++i )
    {
        yssa_block* block = function->blocks.at( i ).get();
        for ( yssa_block* xchandler = block->xchandler;
                        xchandler; xchandler = xchandler->xchandler )
        {
            // Go through entire exception handler chain.
            for ( size_t i = xchandler->lstart; i < xchandler->lfiphi; ++i )
            {
                yssa_opinst* op = function->ops.at( i );
                if ( op->opcode == YSSA_VAR && op->variable )
                {
                    // This variable is used in this handler.
                    yssa_live_range** pnext = &op->variable->live;
                    union_live_range( module, pnext,
                                    block->lstart, block->lfinal );

                    // Don't treat it as an argument.
                    vargof[ op->variable ] = YSSA_NOARG;
                }
            }
        }
    }

    for ( auto argvar : vargof )
    {
        if ( argvar.second != YSSA_NOARG )
        {
            argvar.first->argof = argvar.second;
        }
    }

}

static void successor( yssa_function* function,
                yssa_live* live, yssa_block* prev, yssa_block* block )
{
    // Successor might not exist.
    if ( ! block )
    {
        return;
    }

    // Add all values that were live at the start of this successor.
    // If it's a back reference, we haven't calculated liveness yet.
    if ( prev->index < block->index )
    {
        for ( yssa_opinst* op : block->livein )
        {
            live->emplace( op, prev->lfinal );
        }
    }

    // Find index of this code path in the block block's previous link.
    // This relies on phi-function operands being correctly ordered.
    size_t index = 0;
    for ( ; index < block->prev.size(); ++index )
    {
        if ( block->prev.at( index ) == prev )
        {
            break;
        }
    }
    assert( index < block->prev.size() );

    // Variables that are live at the start of block include those
    // variables referenced by phi-functions coming from this code path.
    for ( size_t i = block->lstart; i < block->lfiphi; ++i )
    {
        yssa_opinst* op = function->ops.at( i );
        assert( op->opcode != YSSA_REF );

        if ( op->opcode == YSSA_PHI )
        {
            live->emplace( op->operand[ index ], prev->lfinal );
        }
    }

}

static void live_loop( yssa_module* module,
                yssa_loop* loop, const yssa_live& live )
{
    // Each op is live during each block of the loop.
    for ( auto liveop : live )
    {
        yssa_opinst* op = liveop.first;
        for ( yssa_block* block : loop->blocks )
        {
            union_live_range( module, &op->live, block->lstart, block->lfinal );
        }
    }

    // And live during any sub-loops.
    for ( yssa_loop* child : loop->children )
    {
        live_loop( module, child, live );
    }
}

static void add_live_range( yssa_module* module,
                yssa_opinst* op, size_t start, size_t final )
{
    assert( ! op->live || op->live->start >= final );
    if ( op->live && op->live->start == final )
    {
        op->live->start = start;
    }
    else
    {
        yssa_live_range* next = op->live;
        op->live = new ( module->alloc ) yssa_live_range( start, final, next );
    }
}

static void union_live_range( yssa_module* module,
                yssa_live_range** pnext, size_t start, size_t final )
{
    // Skip all existing ranges that lie completely before this range.
    while ( *pnext && ( *pnext )->final < start )
    {
        pnext = &( *pnext )->next;
    }

    // The current range is either overlapping, or after.
    if ( *pnext && ( *pnext )->start <= final )
    {
        // Overlapping.
        yssa_live_range* overlap = *pnext;
        overlap->start = std::min( overlap->start, start );
        overlap->final = std::max( overlap->final, final );

        while ( overlap->next && overlap->next->start <= final )
        {
            overlap->final = std::max( overlap->next->final, final );
            overlap->next  = overlap->next->next;
        }
    }
    else
    {
        // Next range is after, so insert new range.
        *pnext = new ( module->alloc ) yssa_live_range( start, final, *pnext );
    }
}

<commit_msg>operands to non-call ops must have argof cleared<commit_after>//
//  yssa_liveness.cpp
//
//  Created by Edmund Kapusniak on 29/03/2015.
//  Copyright (c) 2015 Edmund Kapusniak. All rights reserved.
//

#include "yssa_liveness.h"
#include "yssa.h"

/*

    Useful papers about liveness analysis:

    http://www.christianwimmer.at/Publications/Wimmer10a/Wimmer10a.pdf

    Linear Scan Register Allocation on SSA Form
        by Christian Wimmer, Michael Franz

    http://hal.archives-ouvertes.fr/docs/00/55/85/09/PDF/RR-7503.pdf

    Computing Liveness Sets for SSA-Form Programs (2011)
        by Florian Brandner, Benoit Boissinot, Alain Darte,
            Benoît Dupont De Dinechin, Fabrice Rastello

*/

/*
    We work on linearized SSA with analysis information.  Live ranges are
    represented as ranges of op indexes in the linearized SSA.

    Live ranges for each op and each variable are calculated.

     -  Our modified SSA form mandates that the live ranges of the definitions
            of a variable do not overlap.
     -  Live ranges can have holes, where ops are not live in certain blocks.
     -  Variables references from exception handlers are live throughout all
            blocks protected by that handler.

*/

typedef std::unordered_map< yssa_opinst*, size_t > yssa_live;

static void successor( yssa_function* function,
                yssa_live* live, yssa_block* prev, yssa_block* block );
static void live_loop( yssa_module* module,
                yssa_loop* loop, const yssa_live& live );

static void add_live_range( yssa_module* module,
                yssa_opinst* op, size_t start, size_t final );
static void union_live_range( yssa_module* module,
                yssa_live_range** pnext, size_t start, size_t final );

static yssa_opinst* YSSA_NOARG = (yssa_opinst*)-1;

void yssa_liveness( yssa_module* module, yssa_function* function )
{
    // Map opinsts to the single call op they are an argument for, or NOARG.

    std::unordered_map< yssa_opinst*, yssa_opinst* > argof;

    // Calculate live ranges for each op.

    for ( int i = (int)function->blocks.size() - 1; i >= 0; --i )
    {
        yssa_block* block = function->blocks.at( i ).get();

        // The end of the live range for each op.
        std::unordered_map< yssa_opinst*, size_t > live;

        // At the bottom of the block, all variables that are live on
        // entry to successor blocks are live.  This includes all
        // references in phi-functions from this code path.
        if ( block->test )
        {
            successor( function, &live, block, block->next );
            successor( function, &live, block, block->fail );

            // The condition is also live at the bottom of the block.
            live.emplace( block->test, block->lfinal );
        }
        else
        {
            successor( function, &live, block, block->next );
        }

        // Visit each op in the block, building liveness information.
        for ( size_t i = block->lfinal; i > block->lstart; --i )
        {
            size_t index = i - 1;
            yssa_opinst* op = function->ops.at( index );

            // If this op is live, then this is the start of its live range.
            auto j = live.find( op );
            if ( j != live.end() )
            {
                // Add range.
                add_live_range( module, op, index, j->second );

                // Op is no longer live.
                live.erase( j );
            }

            // Phi function operands do not create liveness here, but
            // rather at the end of predecessor blocks.
            if ( op->opcode == YSSA_PHI || op->opcode == YSSA_REF )
            {
                continue;
            }

            // All operands which are not already live are live now.
            for ( size_t i = 0; i < op->operand_count; ++i )
            {
                if ( ! op->operand[ i ] )
                    continue;

                live.emplace( op->operand[ i ], index );

                // Update argof.
                yssa_opinst*& call = argof[ op->operand[ i ] ];
                if ( op->is_call() )
                {
                    if ( ! call || call == op )
                        call = op;
                    else
                        call = YSSA_NOARG;
                }
                else
                {
                    call = YSSA_NOARG;
                }
            }

            if ( op->has_multival() && op->multival )
            {
                live.emplace( op->multival, index );

                // Update argof.
                yssa_opinst* call = argof[ op->multival ];
                if ( ! call || call == op )
                    call = op;
                else
                    call = YSSA_NOARG;
            }
        }

        // This is the live in set of the block.  All live ranges
        // end at the top of the block.
        for ( auto liveop : live )
        {
            yssa_opinst* op = liveop.first;
            add_live_range( module, op, block->lstart, liveop.second );
            block->livein.insert( op );
        }

        // If this block is a loop header, then all ops live at the start
        // of the loop are live throughout the entire loop.
        if ( block->loop && block->loop->header == block )
        {
            live_loop( module, block->loop, live );

            // And none of them are treated as arguments.
            for ( auto liveop : live )
            {
                argof[ liveop.first ] = YSSA_NOARG;
            }
        }

    }

    // Update argof map.
    for ( auto argop : argof )
    {
        if ( argop.second != YSSA_NOARG )
        {
            function->argof.insert( argop );
        }
    }

    // Live range for a variable is the union of live ranges of its
    // definitions (which must not overlap), plus the range of all
    // protected blocks if the variable is used in an xchandler.

    std::unordered_map< yssa_variable*, yssa_opinst* > vargof;

    for ( size_t i = 0; i < function->ops.size(); ++i )
    {
        yssa_opinst* op = function->ops.at( i );
        if ( op->live && ! op->has_associated() && op->variable )
        {
            // Merge live range into live range of variable.
            for ( yssa_live_range* live = op->live; live; live = live->next )
            {
                yssa_live_range** pnext = &op->variable->live;
                union_live_range( module, pnext, live->start, live->final );
            }

            // Update argument status.
            yssa_opinst*& call = vargof[ op->variable ];
            auto j = function->argof.find( op );
            if ( j != function->argof.end() )
            {
                if ( ! call || call == j->second )
                    call = j->second;
                else
                    call = YSSA_NOARG;
            }
            else
            {
                call = YSSA_NOARG;
            }
        }
    }

    for ( size_t i = 0; i < function->blocks.size(); ++i )
    {
        yssa_block* block = function->blocks.at( i ).get();
        for ( yssa_block* xchandler = block->xchandler;
                        xchandler; xchandler = xchandler->xchandler )
        {
            // Go through entire exception handler chain.
            for ( size_t i = xchandler->lstart; i < xchandler->lfiphi; ++i )
            {
                yssa_opinst* op = function->ops.at( i );
                if ( op->opcode == YSSA_VAR && op->variable )
                {
                    // This variable is used in this handler.
                    yssa_live_range** pnext = &op->variable->live;
                    union_live_range( module, pnext,
                                    block->lstart, block->lfinal );

                    // Don't treat it as an argument.
                    vargof[ op->variable ] = YSSA_NOARG;
                }
            }
        }
    }

    for ( auto argvar : vargof )
    {
        if ( argvar.second != YSSA_NOARG )
        {
            argvar.first->argof = argvar.second;
        }
    }

}

static void successor( yssa_function* function,
                yssa_live* live, yssa_block* prev, yssa_block* block )
{
    // Successor might not exist.
    if ( ! block )
    {
        return;
    }

    // Add all values that were live at the start of this successor.
    // If it's a back reference, we haven't calculated liveness yet.
    if ( prev->index < block->index )
    {
        for ( yssa_opinst* op : block->livein )
        {
            live->emplace( op, prev->lfinal );
        }
    }

    // Find index of this code path in the block block's previous link.
    // This relies on phi-function operands being correctly ordered.
    size_t index = 0;
    for ( ; index < block->prev.size(); ++index )
    {
        if ( block->prev.at( index ) == prev )
        {
            break;
        }
    }
    assert( index < block->prev.size() );

    // Variables that are live at the start of block include those
    // variables referenced by phi-functions coming from this code path.
    for ( size_t i = block->lstart; i < block->lfiphi; ++i )
    {
        yssa_opinst* op = function->ops.at( i );
        assert( op->opcode != YSSA_REF );

        if ( op->opcode == YSSA_PHI )
        {
            live->emplace( op->operand[ index ], prev->lfinal );
        }
    }

}

static void live_loop( yssa_module* module,
                yssa_loop* loop, const yssa_live& live )
{
    // Each op is live during each block of the loop.
    for ( auto liveop : live )
    {
        yssa_opinst* op = liveop.first;
        for ( yssa_block* block : loop->blocks )
        {
            union_live_range( module, &op->live, block->lstart, block->lfinal );
        }
    }

    // And live during any sub-loops.
    for ( yssa_loop* child : loop->children )
    {
        live_loop( module, child, live );
    }
}

static void add_live_range( yssa_module* module,
                yssa_opinst* op, size_t start, size_t final )
{
    assert( ! op->live || op->live->start >= final );
    if ( op->live && op->live->start == final )
    {
        op->live->start = start;
    }
    else
    {
        yssa_live_range* next = op->live;
        op->live = new ( module->alloc ) yssa_live_range( start, final, next );
    }
}

static void union_live_range( yssa_module* module,
                yssa_live_range** pnext, size_t start, size_t final )
{
    // Skip all existing ranges that lie completely before this range.
    while ( *pnext && ( *pnext )->final < start )
    {
        pnext = &( *pnext )->next;
    }

    // The current range is either overlapping, or after.
    if ( *pnext && ( *pnext )->start <= final )
    {
        // Overlapping.
        yssa_live_range* overlap = *pnext;
        overlap->start = std::min( overlap->start, start );
        overlap->final = std::max( overlap->final, final );

        while ( overlap->next && overlap->next->start <= final )
        {
            overlap->final = std::max( overlap->next->final, final );
            overlap->next  = overlap->next->next;
        }
    }
    else
    {
        // Next range is after, so insert new range.
        *pnext = new ( module->alloc ) yssa_live_range( start, final, *pnext );
    }
}

<|endoftext|>


<commit_before>/* Copyright 2020 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

// Reference implementation of the DebugLog() function that's required for a
// platform to support the TensorFlow Lite for Microcontrollers library. This is
// the only function that's absolutely required to be available on a target
// device, since it's used for communicating test results back to the host so
// that we can verify the implementation is working correctly.
// It's designed to be as easy as possible to supply an implementation though.
// On platforms that have a POSIX stack or C library, it can be written as a
// single call to `fprintf(stderr, "%s", s)` to output a string to the error
// stream of the console, but if there's no OS or C library available, there's
// almost always an equivalent way to write out a string to some serial
// interface that can be used instead. For example on Arm M-series MCUs, calling
// the `bkpt #0xAB` assembler instruction will output the string in r1 to
// whatever debug serial connection is available. If you're running mbed, you
// can do the same by creating `Serial pc(USBTX, USBRX)` and then calling
// `pc.printf("%s", s)`.
// To add an equivalent function for your own platform, create your own
// implementation file, and place it in a subfolder with named after the OS
// you're targeting. For example, see the Cortex M bare metal version in
// tensorflow/lite/micro/bluepill/debug_log.cc or the mbed one on
// tensorflow/lite/micro/mbed/debug_log.cc.

#include "tensorflow/lite/micro/debug_log.h"

#ifndef TF_LITE_STRIP_ERROR_STRINGS
#include <cstdio>
#endif

extern "C" void DebugLog(const char* s) {
#ifndef TF_LITE_STRIP_ERROR_STRINGS
  // Reusing TF_LITE_STRIP_ERROR_STRINGS to disable DebugLog completely to get
  // maximum reduction in binary size. This is because we have DebugLog calls
  // via TF_LITE_CHECK that are not stubbed out by TF_LITE_REPORT_ERROR.
  //fprintf(stderr, "%s", s);
  puts(s);
#endif
}
<commit_msg>debug_log.cc: Add a short delay to debug output<commit_after>/* Copyright 2020 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

// Reference implementation of the DebugLog() function that's required for a
// platform to support the TensorFlow Lite for Microcontrollers library. This is
// the only function that's absolutely required to be available on a target
// device, since it's used for communicating test results back to the host so
// that we can verify the implementation is working correctly.
// It's designed to be as easy as possible to supply an implementation though.
// On platforms that have a POSIX stack or C library, it can be written as a
// single call to `fprintf(stderr, "%s", s)` to output a string to the error
// stream of the console, but if there's no OS or C library available, there's
// almost always an equivalent way to write out a string to some serial
// interface that can be used instead. For example on Arm M-series MCUs, calling
// the `bkpt #0xAB` assembler instruction will output the string in r1 to
// whatever debug serial connection is available. If you're running mbed, you
// can do the same by creating `Serial pc(USBTX, USBRX)` and then calling
// `pc.printf("%s", s)`.
// To add an equivalent function for your own platform, create your own
// implementation file, and place it in a subfolder with named after the OS
// you're targeting. For example, see the Cortex M bare metal version in
// tensorflow/lite/micro/bluepill/debug_log.cc or the mbed one on
// tensorflow/lite/micro/mbed/debug_log.cc.

#include "tensorflow/lite/micro/debug_log.h"

#ifndef TF_LITE_STRIP_ERROR_STRINGS
#include <cstdio>
#endif

extern "C" void DebugLog(const char* s) {
#ifndef TF_LITE_STRIP_ERROR_STRINGS
  // Reusing TF_LITE_STRIP_ERROR_STRINGS to disable DebugLog completely to get
  // maximum reduction in binary size. This is because we have DebugLog calls
  // via TF_LITE_CHECK that are not stubbed out by TF_LITE_REPORT_ERROR.
  // fprintf(stderr, "%s", s);
  puts(s);

  // Insert short delay to allow UART to catch up
  for (int i = 0; i < 50000; i++) {
    // nop
    __asm__ __volatile__("add zero, zero, zero");
  }
#endif
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2012-2015 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif

#include "gazebo/sensors/DepthCameraSensor.hh"
#include "gazebo_opticalFlow_plugin.h"

#include <cv.h>
#include <highgui.h>
#include <math.h>
#include <string>
#include <iostream>

using namespace cv;
using namespace std;

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(CameraPlugin)

/////////////////////////////////////////////////
CameraPlugin::CameraPlugin()
: SensorPlugin(), width(0), height(0), depth(0)
{
}

/////////////////////////////////////////////////
CameraPlugin::~CameraPlugin()
{
  this->parentSensor.reset();
  this->camera.reset();
}

/////////////////////////////////////////////////
void CameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
  if (!_sensor)
    gzerr << "Invalid sensor pointer.\n";

  this->parentSensor =
#if GAZEBO_MAJOR_VERSION >= 7
    std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#else
    boost::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#endif

  if (!this->parentSensor)
  {
    gzerr << "CameraPlugin requires a CameraSensor.\n";
#if GAZEBO_MAJOR_VERSION >= 7
    if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#else
    if (boost::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#endif
      gzmsg << "It is a depth camera sensor\n";
  }

  this->camera = this->parentSensor->GetCamera();

  if (!this->parentSensor)
  {
    gzerr << "CameraPlugin not attached to a camera sensor\n";
    return;
  }

  this->width = this->camera->GetImageWidth();
  this->height = this->camera->GetImageHeight();
  this->depth = this->camera->GetImageDepth();
  this->format = this->camera->GetImageFormat();

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_optical_flow_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);
  opticalFlow_pub_ = node_handle_->Advertise<opticalFlow_msgs::msgs::opticalFlow>(topicName, 10);

  this->newFrameConnection = this->camera->ConnectNewImageFrame(
      boost::bind(&CameraPlugin::OnNewFrame, this, _1, this->width, this->height, this->depth, this->format));

  this->parentSensor->SetActive(true);
}

/////////////////////////////////////////////////
void CameraPlugin::OnNewFrame(const unsigned char * _image,
                              unsigned int _width,
                              unsigned int _height,
                              unsigned int _depth,
                              const std::string &_format)
{
  _image = this->camera->GetImageData(0);
  //GetHFOV gives fucking gazebo::math::Angle which you can not cast...
  const double Hfov = 0.6;
  const double focal_length = (_width/2)/tan(Hfov/2);

  double pixel_flow_x_integral = 0.0;
  double pixel_flow_y_integral = 0.0;
  double rate = this->camera->GetRenderRate();
  if (!isfinite(rate))
	   rate =  30.0;
  double dt = 1.0 / rate;

  Mat frame = Mat(_height, _width, CV_8UC3);
  Mat frameBGR = Mat(_height, _width, CV_8UC3);
  frame.data = (uchar*)_image; //frame is not the right color now -> convert
  cvtColor(frame, frameBGR, CV_RGB2BGR);
  cvtColor(frameBGR, frame_gray, CV_BGR2GRAY);

  featuresPrevious = featuresCurrent;

  goodFeaturesToTrack(frame_gray, featuresCurrent, maxfeatures, qualityLevel, minDistance, Mat(), blockSize, useHarrisDetector, k); //calculate the features

  if (!old_gray.empty() && featuresPrevious.size() > 0){
    calcOpticalFlowPyrLK(old_gray, frame_gray, featuresPrevious, featuresNextPos, featuresFound, err);
  }

  /*/// Set the needed parameters to find the refined corners
  Size winSize = Size( 5, 5 );
  Size zeroZone = Size( -1, -1 );
  TermCriteria criteria = TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001 );

  // Calculate the refined corner locations
  cornerSubPix(frame_gray, featuresNextPos, winSize, zeroZone, criteria);
  cornerSubPix(old_gray, featuresPrevious, winSize, zeroZone, criteria);*/

  //calc diff
  int meancount = 0;
  for (int i = 0; i < featuresNextPos.size(); i++) {

    if (featuresFound[i] == true) {
     	pixel_flow_x_integral += featuresNextPos[i].x - featuresPrevious[i].x;
      pixel_flow_y_integral += featuresNextPos[i].y - featuresPrevious[i].y;
      meancount++;
	  }
  }

  double flow_x_ang = atan2(pixel_flow_x_integral/meancount, focal_length);
  double flow_y_ang = atan2(pixel_flow_y_integral/meancount, focal_length);

  old_gray = frame_gray.clone();

  //std::cout << "pixel flow x = " << pixel_flow_x_integral << "   pixel flow y = " << pixel_flow_y_integral << endl;

  opticalFlow_message.set_time_usec(100000000000000);//big number to prevent timeout in inav
  opticalFlow_message.set_sensor_id(2.0);
  opticalFlow_message.set_integration_time_us(dt * 1000000);
  opticalFlow_message.set_integrated_x(flow_x_ang);
  opticalFlow_message.set_integrated_y(flow_y_ang);
  opticalFlow_message.set_integrated_xgyro(0.0); //get real values in gazebo_mavlink_interface.cpp
  opticalFlow_message.set_integrated_ygyro(0.0); //get real values in gazebo_mavlink_interface.cpp
  opticalFlow_message.set_integrated_zgyro(0.0); //get real values in gazebo_mavlink_interface.cpp
  opticalFlow_message.set_temperature(20.0);
  opticalFlow_message.set_quality(meancount * 255 / maxfeatures); //features?
  opticalFlow_message.set_time_delta_distance_us(0.0);
  opticalFlow_message.set_distance(0.0); //get real values in gazebo_mavlink_interface.cpp

  opticalFlow_pub_->Publish(opticalFlow_message);

}
<commit_msg>gazebo_opticalFlow_plugin: fix compilation warnings against Gazebo 7<commit_after>/*
 * Copyright (C) 2012-2015 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif

#include "gazebo/sensors/DepthCameraSensor.hh"
#include "gazebo_opticalFlow_plugin.h"

#include <cv.h>
#include <highgui.h>
#include <math.h>
#include <string>
#include <iostream>

using namespace cv;
using namespace std;

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(CameraPlugin)

/////////////////////////////////////////////////
CameraPlugin::CameraPlugin()
: SensorPlugin(), width(0), height(0), depth(0)
{
}

/////////////////////////////////////////////////
CameraPlugin::~CameraPlugin()
{
  this->parentSensor.reset();
  this->camera.reset();
}

/////////////////////////////////////////////////
void CameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{
  if (!_sensor)
    gzerr << "Invalid sensor pointer.\n";

  this->parentSensor =
#if GAZEBO_MAJOR_VERSION >= 7
    std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#else
    boost::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#endif

  if (!this->parentSensor)
  {
    gzerr << "CameraPlugin requires a CameraSensor.\n";
#if GAZEBO_MAJOR_VERSION >= 7
    if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#else
    if (boost::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#endif
      gzmsg << "It is a depth camera sensor\n";
  }

#if GAZEBO_MAJOR_VERSION >= 7
  this->camera = this->parentSensor->Camera();
#else
  this->camera = this->parentSensor->GetCamera();
#endif

  if (!this->parentSensor)
  {
    gzerr << "CameraPlugin not attached to a camera sensor\n";
    return;
  }

#if GAZEBO_MAJOR_VERSION >= 7
  this->width = this->camera->ImageWidth();
  this->height = this->camera->ImageHeight();
  this->depth = this->camera->ImageDepth();
  this->format = this->camera->ImageFormat();
#else
  this->width = this->camera->GetImageWidth();
  this->height = this->camera->GetImageHeight();
  this->depth = this->camera->GetImageDepth();
  this->format = this->camera->GetImageFormat();
#endif

  if (_sdf->HasElement("robotNamespace"))
    namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
  else
    gzwarn << "[gazebo_optical_flow_plugin] Please specify a robotNamespace.\n";

  node_handle_ = transport::NodePtr(new transport::Node());
  node_handle_->Init(namespace_);
  opticalFlow_pub_ = node_handle_->Advertise<opticalFlow_msgs::msgs::opticalFlow>(topicName, 10);

  this->newFrameConnection = this->camera->ConnectNewImageFrame(
      boost::bind(&CameraPlugin::OnNewFrame, this, _1, this->width, this->height, this->depth, this->format));

  this->parentSensor->SetActive(true);
}

/////////////////////////////////////////////////
void CameraPlugin::OnNewFrame(const unsigned char * _image,
                              unsigned int _width,
                              unsigned int _height,
                              unsigned int _depth,
                              const std::string &_format)
{
#if GAZEBO_MAJOR_VERSION >= 7
  _image = this->camera->ImageData(0);
#else
  _image = this->camera->GetImageData(0);
#endif
  //GetHFOV gives fucking gazebo::math::Angle which you can not cast...
  const double Hfov = 0.6;
  const double focal_length = (_width/2)/tan(Hfov/2);

  double pixel_flow_x_integral = 0.0;
  double pixel_flow_y_integral = 0.0;
#if GAZEBO_MAJOR_VERSION >= 7
  double rate = this->camera->RenderRate();
#else
  double rate = this->camera->GetRenderRate();
#endif
  if (!isfinite(rate))
	   rate =  30.0;
  double dt = 1.0 / rate;

  Mat frame = Mat(_height, _width, CV_8UC3);
  Mat frameBGR = Mat(_height, _width, CV_8UC3);
  frame.data = (uchar*)_image; //frame is not the right color now -> convert
  cvtColor(frame, frameBGR, CV_RGB2BGR);
  cvtColor(frameBGR, frame_gray, CV_BGR2GRAY);

  featuresPrevious = featuresCurrent;

  goodFeaturesToTrack(frame_gray, featuresCurrent, maxfeatures, qualityLevel, minDistance, Mat(), blockSize, useHarrisDetector, k); //calculate the features

  if (!old_gray.empty() && featuresPrevious.size() > 0){
    calcOpticalFlowPyrLK(old_gray, frame_gray, featuresPrevious, featuresNextPos, featuresFound, err);
  }

  /*/// Set the needed parameters to find the refined corners
  Size winSize = Size( 5, 5 );
  Size zeroZone = Size( -1, -1 );
  TermCriteria criteria = TermCriteria( CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 40, 0.001 );

  // Calculate the refined corner locations
  cornerSubPix(frame_gray, featuresNextPos, winSize, zeroZone, criteria);
  cornerSubPix(old_gray, featuresPrevious, winSize, zeroZone, criteria);*/

  //calc diff
  int meancount = 0;
  for (int i = 0; i < featuresNextPos.size(); i++) {

    if (featuresFound[i] == true) {
     	pixel_flow_x_integral += featuresNextPos[i].x - featuresPrevious[i].x;
      pixel_flow_y_integral += featuresNextPos[i].y - featuresPrevious[i].y;
      meancount++;
	  }
  }

  double flow_x_ang = atan2(pixel_flow_x_integral/meancount, focal_length);
  double flow_y_ang = atan2(pixel_flow_y_integral/meancount, focal_length);

  old_gray = frame_gray.clone();

  //std::cout << "pixel flow x = " << pixel_flow_x_integral << "   pixel flow y = " << pixel_flow_y_integral << endl;

  opticalFlow_message.set_time_usec(100000000000000);//big number to prevent timeout in inav
  opticalFlow_message.set_sensor_id(2.0);
  opticalFlow_message.set_integration_time_us(dt * 1000000);
  opticalFlow_message.set_integrated_x(flow_x_ang);
  opticalFlow_message.set_integrated_y(flow_y_ang);
  opticalFlow_message.set_integrated_xgyro(0.0); //get real values in gazebo_mavlink_interface.cpp
  opticalFlow_message.set_integrated_ygyro(0.0); //get real values in gazebo_mavlink_interface.cpp
  opticalFlow_message.set_integrated_zgyro(0.0); //get real values in gazebo_mavlink_interface.cpp
  opticalFlow_message.set_temperature(20.0);
  opticalFlow_message.set_quality(meancount * 255 / maxfeatures); //features?
  opticalFlow_message.set_time_delta_distance_us(0.0);
  opticalFlow_message.set_distance(0.0); //get real values in gazebo_mavlink_interface.cpp

  opticalFlow_pub_->Publish(opticalFlow_message);

}
<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
//
// Copyright 2006 - 2014, Paul Beckingham, Federico Hernandez.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// http://www.opensource.org/licenses/mit-license.php
//
////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <Context.h>
#include <A3t.h>

Context context;

////////////////////////////////////////////////////////////////////////////////
int main (int argc, const char** argv)
{
  try
  {
    // Prepare the Context object.

    A3t a3t;
    a3t.initialize (argc, argv);

    // Reports.
    a3t.entity ("report",     "list");
    a3t.entity ("report",     "next");

    // Read-only commands.
    a3t.entity ("readcmd",    "export");
    a3t.entity ("readcmd",    "info");
    a3t.entity ("readcmd",    "list");
    a3t.entity ("readcmd",    "next");
    a3t.entity ("readcmd",    "projects");

    // Write commands.
    a3t.entity ("writecmd",   "add");
    a3t.entity ("writecmd",   "annotate");
    a3t.entity ("writecmd",   "delete");
    a3t.entity ("writecmd",   "denotate");
    a3t.entity ("writecmd",   "done");
    a3t.entity ("writecmd",   "modify");
    a3t.entity ("writecmd",   "start");
    a3t.entity ("writecmd",   "stop");

    // Special commands.
    a3t.entity ("specialcmd", "calendar");
    a3t.entity ("specialcmd", "edit");
    a3t.entity ("writecmd",   "import");

    // Helper commands.
    a3t.entity ("helper",     "_get");
    a3t.entity ("helper",     "_query");
    a3t.entity ("helper",     "_ids");
    a3t.entity ("helper",     "_uuids");
    a3t.entity ("helper",     "_zshids");
    a3t.entity ("helper",     "_zshuuids");

    // Attributes (columns).
    a3t.entity ("attribute",  "description");
    a3t.entity ("attribute",  "due");
    a3t.entity ("attribute",  "duration"); // UDAs are included.
    a3t.entity ("attribute",  "end");
    a3t.entity ("attribute",  "entry");
    a3t.entity ("attribute",  "priority");
    a3t.entity ("attribute",  "project");
    a3t.entity ("attribute",  "scheduled");
    a3t.entity ("attribute",  "start");
    a3t.entity ("attribute",  "uuid");
    a3t.entity ("attribute",  "wait");

    // Pseudo-attributes.
    a3t.entity ("pseudo",     "limit");

    // UDAs.
    a3t.entity ("uda",        "duration");

    // Modifiers.
    a3t.entity ("modifier",   "before");
    a3t.entity ("modifier",   "under");
    a3t.entity ("modifier",   "below");
    a3t.entity ("modifier",   "after");
    a3t.entity ("modifier",   "over");
    a3t.entity ("modifier",   "above");
    a3t.entity ("modifier",   "none");
    a3t.entity ("modifier",   "any");
    a3t.entity ("modifier",   "is");
    a3t.entity ("modifier",   "equals");
    a3t.entity ("modifier",   "isnt");
    a3t.entity ("modifier",   "not");
    a3t.entity ("modifier",   "has");
    a3t.entity ("modifier",   "contains");
    a3t.entity ("modifier",   "hasnt");
    a3t.entity ("modifier",   "startswith");
    a3t.entity ("modifier",   "left");
    a3t.entity ("modifier",   "endswith");
    a3t.entity ("modifier",   "right");
    a3t.entity ("modifier",   "word");
    a3t.entity ("modifier",   "noword");

    // Operators.
    a3t.entity ("operator",   "and");
    a3t.entity ("operator",   "or");
    a3t.entity ("operator",   "xor");
    a3t.entity ("operator",   "<=");
    a3t.entity ("operator",   ">=");
    a3t.entity ("operator",   "!~");
    a3t.entity ("operator",   "!=");
    a3t.entity ("operator",   "==");
    a3t.entity ("operator",   "=");
    a3t.entity ("operator",   ">");
    a3t.entity ("operator",   "~");
    a3t.entity ("operator",   "!");
    a3t.entity ("operator",   "_hastag_");
    a3t.entity ("operator",   "_notag_");
    a3t.entity ("operator",   "-");
    a3t.entity ("operator",   "*");
    a3t.entity ("operator",   "/");
    a3t.entity ("operator",   "+");
    a3t.entity ("operator",   "-");
    a3t.entity ("operator",   "<");
    a3t.entity ("operator",   "(");
    a3t.entity ("operator",   ")");
    a3t.entity ("operator",   "%");
    a3t.entity ("operator",   "^");
    a3t.entity ("operator",   "!");

    a3t.findFileOverride ();
    a3t.findConfigOverride ();

    Tree* tree = a3t.parse ();
    if (tree)
      std::cout << tree->dump ();
  }

  catch (const std::string& error)
  {
    std::cout << "Error: " << error << std::endl;
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
<commit_msg>Args<commit_after>////////////////////////////////////////////////////////////////////////////////
//
// Copyright 2006 - 2014, Paul Beckingham, Federico Hernandez.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.
//
// http://www.opensource.org/licenses/mit-license.php
//
////////////////////////////////////////////////////////////////////////////////

#include <iostream>
#include <Context.h>
#include <A3t.h>

Context context;

////////////////////////////////////////////////////////////////////////////////
int main (int argc, const char** argv)
{
  try
  {
    A3t a3t;
    a3t.initialize (argc, argv);
    a3t.append_stdin ();
    a3t.findFileOverride ();
    a3t.findConfigOverride ();
    a3t.findAliases ();

    // Reports.
    a3t.entity ("report",     "list");
    a3t.entity ("report",     "next");

    // Read-only commands.
    a3t.entity ("readcmd",    "export");
    a3t.entity ("readcmd",    "info");
    a3t.entity ("readcmd",    "list");
    a3t.entity ("readcmd",    "next");
    a3t.entity ("readcmd",    "projects");

    // Write commands.
    a3t.entity ("writecmd",   "add");
    a3t.entity ("writecmd",   "annotate");
    a3t.entity ("writecmd",   "delete");
    a3t.entity ("writecmd",   "denotate");
    a3t.entity ("writecmd",   "done");
    a3t.entity ("writecmd",   "modify");
    a3t.entity ("writecmd",   "start");
    a3t.entity ("writecmd",   "stop");

    // Special commands.
    a3t.entity ("specialcmd", "calendar");
    a3t.entity ("specialcmd", "edit");
    a3t.entity ("writecmd",   "import");

    // Helper commands.
    a3t.entity ("helper",     "_get");
    a3t.entity ("helper",     "_query");
    a3t.entity ("helper",     "_ids");
    a3t.entity ("helper",     "_uuids");
    a3t.entity ("helper",     "_zshids");
    a3t.entity ("helper",     "_zshuuids");

    // Attributes (columns).
    a3t.entity ("attribute",  "description");
    a3t.entity ("attribute",  "due");
    a3t.entity ("attribute",  "duration"); // UDAs are included.
    a3t.entity ("attribute",  "end");
    a3t.entity ("attribute",  "entry");
    a3t.entity ("attribute",  "priority");
    a3t.entity ("attribute",  "project");
    a3t.entity ("attribute",  "scheduled");
    a3t.entity ("attribute",  "start");
    a3t.entity ("attribute",  "uuid");
    a3t.entity ("attribute",  "wait");

    // Pseudo-attributes.
    a3t.entity ("pseudo",     "limit");

    // UDAs.
    a3t.entity ("uda",        "duration");

    // Modifiers.
    a3t.entity ("modifier",   "before");
    a3t.entity ("modifier",   "under");
    a3t.entity ("modifier",   "below");
    a3t.entity ("modifier",   "after");
    a3t.entity ("modifier",   "over");
    a3t.entity ("modifier",   "above");
    a3t.entity ("modifier",   "none");
    a3t.entity ("modifier",   "any");
    a3t.entity ("modifier",   "is");
    a3t.entity ("modifier",   "equals");
    a3t.entity ("modifier",   "isnt");
    a3t.entity ("modifier",   "not");
    a3t.entity ("modifier",   "has");
    a3t.entity ("modifier",   "contains");
    a3t.entity ("modifier",   "hasnt");
    a3t.entity ("modifier",   "startswith");
    a3t.entity ("modifier",   "left");
    a3t.entity ("modifier",   "endswith");
    a3t.entity ("modifier",   "right");
    a3t.entity ("modifier",   "word");
    a3t.entity ("modifier",   "noword");

    // Operators.
    a3t.entity ("operator",   "and");
    a3t.entity ("operator",   "or");
    a3t.entity ("operator",   "xor");
    a3t.entity ("operator",   "<=");
    a3t.entity ("operator",   ">=");
    a3t.entity ("operator",   "!~");
    a3t.entity ("operator",   "!=");
    a3t.entity ("operator",   "==");
    a3t.entity ("operator",   "=");
    a3t.entity ("operator",   ">");
    a3t.entity ("operator",   "~");
    a3t.entity ("operator",   "!");
    a3t.entity ("operator",   "_hastag_");
    a3t.entity ("operator",   "_notag_");
    a3t.entity ("operator",   "-");
    a3t.entity ("operator",   "*");
    a3t.entity ("operator",   "/");
    a3t.entity ("operator",   "+");
    a3t.entity ("operator",   "-");
    a3t.entity ("operator",   "<");
    a3t.entity ("operator",   "(");
    a3t.entity ("operator",   ")");
    a3t.entity ("operator",   "%");
    a3t.entity ("operator",   "^");
    a3t.entity ("operator",   "!");

    a3t.findCommand ();
    a3t.findUUIDList ();
    a3t.findIdSequence ();
    a3t.inject_defaults ();

    Tree* tree = a3t.parse ();
    if (tree)
      std::cout << tree->dump ();
  }

  catch (const std::string& error)
  {
    std::cout << "Error: " << error << std::endl;
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////
<|endoftext|>


<commit_before>// Copyright (c) 2009 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "config.h"

#include <wtf/HashSet.h>
#include <wtf/RefPtr.h>
#include <wtf/Vector.h>

#include "Document.h"
#include "Frame.h"
#include "Page.h"
#include "V8Binding.h"
#include "V8DOMWindow.h"
#include "V8Index.h"
#include "V8Proxy.h"
#undef LOG

#include "grit/webkit_resources.h"
#include "webkit/api/src/WebViewImpl.h"
#include "webkit/glue/devtools/debugger_agent_impl.h"
#include "webkit/glue/devtools/debugger_agent_manager.h"
#include "webkit/glue/glue_util.h"
#include "webkit/glue/webdevtoolsagent_impl.h"
#include "webkit/glue/webkit_glue.h"

using WebCore::DOMWindow;
using WebCore::Document;
using WebCore::Frame;
using WebCore::Page;
using WebCore::String;
using WebCore::V8ClassIndex;
using WebCore::V8Custom;
using WebCore::V8DOMWindow;
using WebCore::V8DOMWrapper;
using WebCore::V8Proxy;
using WebKit::WebViewImpl;

DebuggerAgentImpl::DebuggerAgentImpl(
    WebViewImpl* web_view_impl,
    DebuggerAgentDelegate* delegate,
    WebDevToolsAgentImpl* webdevtools_agent)
    : web_view_impl_(web_view_impl),
      delegate_(delegate),
      webdevtools_agent_(webdevtools_agent),
      profiler_log_position_(0) {
  DebuggerAgentManager::DebugAttach(this);
}

DebuggerAgentImpl::~DebuggerAgentImpl() {
  DebuggerAgentManager::DebugDetach(this);
}

void DebuggerAgentImpl::DebugBreak() {
  DebuggerAgentManager::DebugBreak(this);
}

void DebuggerAgentImpl::GetContextId() {
  delegate_->SetContextId(webdevtools_agent_->host_id());
}

void DebuggerAgentImpl::StartProfiling(int flags) {
  v8::HandleScope scope;
  WebCore::Frame* frame = GetPage()->mainFrame();
  ASSERT(V8Proxy::retrieve(GetPage()->mainFrame())->isContextInitialized());
  v8::Context::Scope context_scope(V8Proxy::context(frame));
  v8::V8::ResumeProfilerEx(flags);
}

void DebuggerAgentImpl::StopProfiling(int flags) {
  v8::V8::PauseProfilerEx(flags);
}

void DebuggerAgentImpl::GetActiveProfilerModules() {
  delegate_->DidGetActiveProfilerModules(v8::V8::GetActiveProfilerModules());
}

void DebuggerAgentImpl::GetNextLogLines() {
  static char buffer[65536];
  int read_size = v8::V8::GetLogLines(
      profiler_log_position_, buffer, sizeof(buffer) - 1);
  profiler_log_position_ += read_size;
  buffer[read_size] = '\0';
  delegate_->DidGetNextLogLines(buffer);
}

void DebuggerAgentImpl::DebuggerOutput(const String& command) {
  delegate_->DebuggerOutput(command);
  webdevtools_agent_->ForceRepaint();
}

// static
void DebuggerAgentImpl::CreateUtilityContext(
    Frame* frame,
    v8::Persistent<v8::Context>* context) {
  v8::HandleScope scope;

  // TODO(pfeldman): Validate against Soeren.
  // Set up the DOM window as the prototype of the new global object.
  v8::Handle<v8::Context> window_context =
      V8Proxy::context(frame);
  v8::Handle<v8::Object> window_global = window_context->Global();
  v8::Handle<v8::Object> window_wrapper =
      V8DOMWrapper::lookupDOMWrapper(V8ClassIndex::DOMWINDOW, window_global);

  ASSERT(V8DOMWrapper::convertDOMWrapperToNative<DOMWindow>(window_wrapper) ==
      frame->domWindow());

  // Create a new environment using an empty template for the shadow
  // object.  Reuse the global object if one has been created earlier.
  v8::Handle<v8::ObjectTemplate> global_template =
      V8DOMWindow::GetShadowObjectTemplate();

  // Install a security handler with V8.
  global_template->SetAccessCheckCallbacks(
      V8Custom::v8DOMWindowNamedSecurityCheck,
      V8Custom::v8DOMWindowIndexedSecurityCheck,
      v8::Integer::New(V8ClassIndex::DOMWINDOW));

  *context = v8::Context::New(
      NULL /* no extensions */,
      global_template,
      v8::Handle<v8::Object>());
  v8::Context::Scope context_scope(*context);
  v8::Handle<v8::Object> global = (*context)->Global();

  v8::Handle<v8::String> implicit_proto_string = v8::String::New("__proto__");
  global->Set(implicit_proto_string, window_wrapper);

  // Give the code running in the new context a way to get access to the
  // original context.
  global->Set(v8::String::New("contentWindow"), window_global);

  // Inject javascript into the context.
  base::StringPiece basejs = webkit_glue::GetDataResource(IDR_DEVTOOLS_BASE_JS);
  v8::Script::Compile(v8::String::New(basejs.as_string().c_str()))->Run();

  base::StringPiece injectjs_webkit =
      webkit_glue::GetDataResource(IDR_DEVTOOLS_INJECT_WEBKIT_JS);
  v8::Script::Compile(
      v8::String::New(injectjs_webkit.as_string().c_str()))->Run();
  base::StringPiece inject_dispatchjs = webkit_glue::GetDataResource(
      IDR_DEVTOOLS_INJECT_DISPATCH_JS);
  v8::Script::Compile(v8::String::New(
      inject_dispatchjs.as_string().c_str()))->Run();
}

String DebuggerAgentImpl::ExecuteUtilityFunction(
    v8::Handle<v8::Context> context,
    int call_id,
    const char* object,
    const String &function_name,
    const String& json_args,
    bool async,
    String* exception) {
  v8::HandleScope scope;
  ASSERT(!context.IsEmpty());
  if (context.IsEmpty()) {
    *exception = "No window context.";
    return "";
  }
  v8::Context::Scope context_scope(context);

  DebuggerAgentManager::UtilityContextScope utility_scope;

  v8::Handle<v8::Object> dispatch_object = v8::Handle<v8::Object>::Cast(
      context->Global()->Get(v8::String::New(object)));

  v8::Handle<v8::Value> dispatch_function =
      dispatch_object->Get(v8::String::New("dispatch"));
  ASSERT(dispatch_function->IsFunction());
  v8::Handle<v8::Function> function = v8::Handle<v8::Function>::Cast(dispatch_function);

  v8::Handle<v8::String> function_name_wrapper = v8::Handle<v8::String>(
      v8::String::New(function_name.utf8().data()));
  v8::Handle<v8::String> json_args_wrapper = v8::Handle<v8::String>(
      v8::String::New(json_args.utf8().data()));
  v8::Handle<v8::Number> call_id_wrapper = v8::Handle<v8::Number>(
      v8::Number::New(async ? call_id : 0));

  v8::Handle<v8::Value> args[] = {
    function_name_wrapper,
    json_args_wrapper,
    call_id_wrapper
  };

  v8::TryCatch try_catch;
  v8::Handle<v8::Value> res_obj = function->Call(context->Global(), 3, args);
  if (try_catch.HasCaught()) {
    *exception = WebCore::toWebCoreString(try_catch.Message()->Get());
    return "";
  } else {
    return WebCore::toWebCoreStringWithNullCheck(res_obj);
  }
}

void DebuggerAgentImpl::ExecuteVoidJavaScript(v8::Handle<v8::Context> context) {
  v8::HandleScope scope;
  ASSERT(!context.IsEmpty());
  v8::Context::Scope context_scope(context);
  DebuggerAgentManager::UtilityContextScope utility_scope;

  v8::Handle<v8::Value> function =
      context->Global()->Get(v8::String::New("devtools$$void"));
  ASSERT(function->IsFunction());
  v8::Handle<v8::Value> args[] = {
    v8::Local<v8::Value>()
  };
  v8::Handle<v8::Function>::Cast(function)->Call(context->Global(), 0, args);
}

WebCore::Page* DebuggerAgentImpl::GetPage() {
  return web_view_impl_->page();
}
<commit_msg>DevTools: fix renderer crash when pause is pressed during page load<commit_after>// Copyright (c) 2009 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "config.h"

#include <wtf/HashSet.h>
#include <wtf/RefPtr.h>
#include <wtf/Vector.h>

#include "Document.h"
#include "Frame.h"
#include "Page.h"
#include "V8Binding.h"
#include "V8DOMWindow.h"
#include "V8Index.h"
#include "V8Proxy.h"
#undef LOG

#include "grit/webkit_resources.h"
#include "webkit/api/src/WebViewImpl.h"
#include "webkit/glue/devtools/debugger_agent_impl.h"
#include "webkit/glue/devtools/debugger_agent_manager.h"
#include "webkit/glue/glue_util.h"
#include "webkit/glue/webdevtoolsagent_impl.h"
#include "webkit/glue/webkit_glue.h"

using WebCore::DOMWindow;
using WebCore::Document;
using WebCore::Frame;
using WebCore::Page;
using WebCore::String;
using WebCore::V8ClassIndex;
using WebCore::V8Custom;
using WebCore::V8DOMWindow;
using WebCore::V8DOMWrapper;
using WebCore::V8Proxy;
using WebKit::WebViewImpl;

DebuggerAgentImpl::DebuggerAgentImpl(
    WebViewImpl* web_view_impl,
    DebuggerAgentDelegate* delegate,
    WebDevToolsAgentImpl* webdevtools_agent)
    : web_view_impl_(web_view_impl),
      delegate_(delegate),
      webdevtools_agent_(webdevtools_agent),
      profiler_log_position_(0) {
  DebuggerAgentManager::DebugAttach(this);
}

DebuggerAgentImpl::~DebuggerAgentImpl() {
  DebuggerAgentManager::DebugDetach(this);
}

void DebuggerAgentImpl::DebugBreak() {
  DebuggerAgentManager::DebugBreak(this);
}

void DebuggerAgentImpl::GetContextId() {
  delegate_->SetContextId(webdevtools_agent_->host_id());
}

void DebuggerAgentImpl::StartProfiling(int flags) {
  v8::HandleScope scope;
  WebCore::Frame* frame = GetPage()->mainFrame();
  ASSERT(V8Proxy::retrieve(GetPage()->mainFrame())->isContextInitialized());
  v8::Context::Scope context_scope(V8Proxy::context(frame));
  v8::V8::ResumeProfilerEx(flags);
}

void DebuggerAgentImpl::StopProfiling(int flags) {
  v8::V8::PauseProfilerEx(flags);
}

void DebuggerAgentImpl::GetActiveProfilerModules() {
  delegate_->DidGetActiveProfilerModules(v8::V8::GetActiveProfilerModules());
}

void DebuggerAgentImpl::GetNextLogLines() {
  static char buffer[65536];
  int read_size = v8::V8::GetLogLines(
      profiler_log_position_, buffer, sizeof(buffer) - 1);
  profiler_log_position_ += read_size;
  buffer[read_size] = '\0';
  delegate_->DidGetNextLogLines(buffer);
}

void DebuggerAgentImpl::DebuggerOutput(const String& command) {
  delegate_->DebuggerOutput(command);
  webdevtools_agent_->ForceRepaint();
}

// static
void DebuggerAgentImpl::CreateUtilityContext(
    Frame* frame,
    v8::Persistent<v8::Context>* context) {
  v8::HandleScope scope;

  // TODO(pfeldman): Validate against Soeren.
  // Set up the DOM window as the prototype of the new global object.
  v8::Handle<v8::Context> window_context =
      V8Proxy::context(frame);
  v8::Handle<v8::Object> window_global = window_context->Global();
  v8::Handle<v8::Object> window_wrapper =
      V8DOMWrapper::lookupDOMWrapper(V8ClassIndex::DOMWINDOW, window_global);

  ASSERT(V8DOMWrapper::convertDOMWrapperToNative<DOMWindow>(window_wrapper) ==
      frame->domWindow());

  v8::Handle<v8::ObjectTemplate> global_template = v8::ObjectTemplate::New();

  // Install a security handler with V8.
  global_template->SetAccessCheckCallbacks(
      V8Custom::v8DOMWindowNamedSecurityCheck,
      V8Custom::v8DOMWindowIndexedSecurityCheck,
      v8::Integer::New(V8ClassIndex::DOMWINDOW));

  *context = v8::Context::New(
      NULL /* no extensions */,
      global_template,
      v8::Handle<v8::Object>());
  v8::Context::Scope context_scope(*context);
  v8::Handle<v8::Object> global = (*context)->Global();

  v8::Handle<v8::String> implicit_proto_string = v8::String::New("__proto__");
  global->Set(implicit_proto_string, window_wrapper);

  // Give the code running in the new context a way to get access to the
  // original context.
  global->Set(v8::String::New("contentWindow"), window_global);

  // Inject javascript into the context.
  base::StringPiece basejs = webkit_glue::GetDataResource(IDR_DEVTOOLS_BASE_JS);
  v8::Script::Compile(v8::String::New(basejs.as_string().c_str()))->Run();

  base::StringPiece injectjs_webkit =
      webkit_glue::GetDataResource(IDR_DEVTOOLS_INJECT_WEBKIT_JS);
  v8::Script::Compile(
      v8::String::New(injectjs_webkit.as_string().c_str()))->Run();
  base::StringPiece inject_dispatchjs = webkit_glue::GetDataResource(
      IDR_DEVTOOLS_INJECT_DISPATCH_JS);
  v8::Script::Compile(v8::String::New(
      inject_dispatchjs.as_string().c_str()))->Run();
}

String DebuggerAgentImpl::ExecuteUtilityFunction(
    v8::Handle<v8::Context> context,
    int call_id,
    const char* object,
    const String &function_name,
    const String& json_args,
    bool async,
    String* exception) {
  v8::HandleScope scope;
  ASSERT(!context.IsEmpty());
  if (context.IsEmpty()) {
    *exception = "No window context.";
    return "";
  }
  v8::Context::Scope context_scope(context);

  DebuggerAgentManager::UtilityContextScope utility_scope;

  v8::Handle<v8::Object> dispatch_object = v8::Handle<v8::Object>::Cast(
      context->Global()->Get(v8::String::New(object)));

  v8::Handle<v8::Value> dispatch_function =
      dispatch_object->Get(v8::String::New("dispatch"));
  ASSERT(dispatch_function->IsFunction());
  v8::Handle<v8::Function> function = v8::Handle<v8::Function>::Cast(dispatch_function);

  v8::Handle<v8::String> function_name_wrapper = v8::Handle<v8::String>(
      v8::String::New(function_name.utf8().data()));
  v8::Handle<v8::String> json_args_wrapper = v8::Handle<v8::String>(
      v8::String::New(json_args.utf8().data()));
  v8::Handle<v8::Number> call_id_wrapper = v8::Handle<v8::Number>(
      v8::Number::New(async ? call_id : 0));

  v8::Handle<v8::Value> args[] = {
    function_name_wrapper,
    json_args_wrapper,
    call_id_wrapper
  };

  v8::TryCatch try_catch;
  v8::Handle<v8::Value> res_obj = function->Call(context->Global(), 3, args);
  if (try_catch.HasCaught()) {
    *exception = WebCore::toWebCoreString(try_catch.Message()->Get());
    return "";
  } else {
    return WebCore::toWebCoreStringWithNullCheck(res_obj);
  }
}

void DebuggerAgentImpl::ExecuteVoidJavaScript(v8::Handle<v8::Context> context) {
  v8::HandleScope scope;
  ASSERT(!context.IsEmpty());
  v8::Context::Scope context_scope(context);
  DebuggerAgentManager::UtilityContextScope utility_scope;

  v8::Handle<v8::Value> function =
      context->Global()->Get(v8::String::New("devtools$$void"));
  ASSERT(function->IsFunction());
  v8::Handle<v8::Value> args[] = {
    v8::Local<v8::Value>()
  };
  v8::Handle<v8::Function>::Cast(function)->Call(context->Global(), 0, args);
}

WebCore::Page* DebuggerAgentImpl::GetPage() {
  return web_view_impl_->page();
}
<|endoftext|>