<commit_before>// Copyright 2013-2015 Eric Schkufza, Rahul Sharma, Berkeley Churchill, Stefan Heule
//
// 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.

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>

#include "src/ext/cpputil/include/command_line/command_line.h"
#include "src/ext/cpputil/include/serialize/line_reader.h"
#include "src/ext/cpputil/include/serialize/span_reader.h"
#include "src/ext/cpputil/include/signal/debug_handler.h"
#include "src/ext/cpputil/include/system/terminal.h"

#include "src/state/cpu_states.h"
#include "src/stategen/stategen.h"
#include "tools/args/target.h"
#include "tools/gadgets/sandbox.h"
#include "tools/gadgets/target.h"
#include "tools/io/tunit.h"
#include "tools/ui/console.h"

using namespace cpputil;
using namespace std;
using namespace stoke;

auto& io_opt = Heading::create("I/O options:");
auto& bin = ValueArg<string>::create("bin")
            .usage("<path/to/bin>")
            .description("Executable binary containing function to generate testcases for");
auto& args = ValueArg<string, LineReader<>>::create("args")
             .usage("<arg1 arg2 ... argn>")
             .description("Optional command line arguments to pass to binary")
             .default_val("");
auto& out = ValueArg<string>::create("o")
            .alternate("out")
            .usage("<path/to/file.tc>")
            .description("File to write testcases to (defaults to console if unspecified)")
            .required();

auto& common_opt = Heading::create("Common options:");
auto& max_tc = ValueArg<size_t>::create("max_testcases")
               .usage("<int>")
               .description("The maximum number of testcases to generate")
               .default_val(16);

auto& trace_opt = Heading::create("Trace options:");
auto& fxn = ValueArg<string>::create("fxn")
            .usage("<string>")
            .description("Function to generate testcases for")
            .default_val("main");
auto& begin_line = ValueArg<size_t>::create("begin_line")
                   .usage("<int>")
                   .description("The line number to begin recording from")
                   .default_val(1);
auto& end_lines = ValueArg<vector<size_t>>::create("end_lines")
                  .usage("{ 0 1 ... 9 }")
                  .description("Line number to end recording on; recording always stops on returns")
                  .default_val({});
auto& max_stack = ValueArg<uint64_t>::create("max_stack")
                  .usage("<bytes>")
                  .description("The maximum number of bytes to assume could be stack")
                  .default_val(1024);

auto& autogen_opt = Heading::create("Autogen options:");
auto& max_attempts = ValueArg<uint64_t>::create("max_attempts")
                     .usage("<int>")
                     .description("The maximum number of attempts to make at generating a testcase")
                     .default_val(16);
auto& max_memory = ValueArg<uint64_t>::create("max_memory")
                   .usage("<bytes>")
                   .description("The maximum number of bytes to allocate to stack or heap")
                   .default_val(1024);
auto& stack_size = ValueArg<size_t>::create("stack_size")
                   .usage("<int>")
                   .description("The minimum stack size available to the testcase")
                   .default_val(16);

auto& conv_opt = Heading::create("File conversion options:");
auto& compress = FlagArg::create("compress")
                 .description("Convert testcase file from text to binary");
auto& decompress = FlagArg::create("decompress")
                   .description("Convert testcase file from binary to text");
auto& in = ValueArg<string>::create("in")
           .alternate("i")
           .usage("<path/to/file.tc>")
           .description("Path to testcases file")
           .default_val("in.tc");

int auto_gen() {
  TargetGadget target;
  SandboxGadget sb({});

  StateGen sg(&sb, stack_size.value());
  sg.set_max_attempts(max_attempts.value())
  .set_max_memory(max_stack.value());

  CpuStates tcs;
  for (size_t i = 0, ie = max_tc.value(); i < ie; ++i) {
    CpuState tc;
    if (sg.get(tc, target)) {
      tcs.push_back(tc);
    }
  }

  if (tcs.empty()) {
    Console::error(1) << "Unable to generate testcases!" << endl;
  }

  if (out.value() != "") {
    ofstream ofs(out.value());
    tcs.write_text(ofs);
  } else {
    tcs.write_text(Console::msg());
    Console::msg() << endl;
  }

  return 0;
}

int trace(const string& argv0) {
  string here = argv0;
  here = here.substr(0, here.find_last_of("/") + 1);

  const string pin_path = here + "../src/ext/pin-2.13-62732-gcc.4.4.7-linux/";
  const string so_path = pin_path + "source/tools/stoke/obj-intel64/";

  Terminal term;
  term << pin_path << "pin -injection child -t " << so_path << "testcase.so ";

  term << "-f " << fxn.value() << " ";
  if (out.value() != "") {
    term << "-o " << out.value() << " ";
  }
  term << "-x " << max_stack.value() << " ";
  term << "-n " << max_tc.value() << " ";
  term << "-b " << begin_line.value() << " ";
  term << "-e \" ";
  for (auto e : end_lines.value()) {
    term << e << " ";
  }
  term << "\" ";

  term << " -- " << bin.value() << " " << args.value() << endl;

  return 0;
}

int do_compress() {
  ifstream ifs(in.value());
  if (!ifs.is_open()) {
    Console::error(1) << "Unable to open input file: " << in.value() << "!" << endl;
  }

  CpuStates cs;
  cs.read_text(ifs);
  if (ifs.fail()) {
    Console::error(1) << "Unable to read input file: " << in.value() << "!" << endl;
  }

  if (out.value() != "") {
    ofstream ofs(out.value());
    cs.write_bin(ofs);
  } else {
    cs.write_bin(Console::msg());
    Console::msg() << endl;
  }

  return 0;
}

int do_decompress() {
  ifstream ifs(in.value());
  if (!ifs.is_open()) {
    Console::error(1) << "Unable to open input file: " << in.value() << "!" << endl;
  }

  CpuStates cs;
  cs.read_bin(ifs);
  if (ifs.fail()) {
    Console::error(1) << "Unable to read input file: " << in.value() << "!" << endl;
  }

  if (out.value() != "") {
    ofstream ofs(out.value());
    cs.write_text(ofs);
  } else {
    cs.write_text(Console::msg());
    Console::msg() << endl;
  }

  return 0;
}

int main(int argc, char** argv) {
  target_arg.required(false);
  CommandLineConfig::strict_with_convenience(argc, argv);
  DebugHandler::install_sigsegv();
  DebugHandler::install_sigill();

  if (compress.value()) {
    return do_compress();
  } else if (decompress.value()) {
    return do_decompress();
  } else if (target_arg.has_been_provided()) {
    return auto_gen();
  } else {
    return trace(argv[0]);
  }
}

<commit_msg>--out in stoke_testcase is not required<commit_after>// Copyright 2013-2015 Eric Schkufza, Rahul Sharma, Berkeley Churchill, Stefan Heule
//
// 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.

#include <chrono>
#include <fstream>
#include <iostream>
#include <random>
#include <string>
#include <vector>

#include "src/ext/cpputil/include/command_line/command_line.h"
#include "src/ext/cpputil/include/serialize/line_reader.h"
#include "src/ext/cpputil/include/serialize/span_reader.h"
#include "src/ext/cpputil/include/signal/debug_handler.h"
#include "src/ext/cpputil/include/system/terminal.h"

#include "src/state/cpu_states.h"
#include "src/stategen/stategen.h"
#include "tools/args/target.h"
#include "tools/gadgets/sandbox.h"
#include "tools/gadgets/target.h"
#include "tools/io/tunit.h"
#include "tools/ui/console.h"

using namespace cpputil;
using namespace std;
using namespace stoke;

auto& io_opt = Heading::create("I/O options:");
auto& bin = ValueArg<string>::create("bin")
            .usage("<path/to/bin>")
            .description("Executable binary containing function to generate testcases for");
auto& args = ValueArg<string, LineReader<>>::create("args")
             .usage("<arg1 arg2 ... argn>")
             .description("Optional command line arguments to pass to binary")
             .default_val("");
auto& out = ValueArg<string>::create("o")
            .alternate("out")
            .usage("<path/to/file.tc>")
            .description("File to write testcases to (defaults to console if unspecified)");

auto& common_opt = Heading::create("Common options:");
auto& max_tc = ValueArg<size_t>::create("max_testcases")
               .usage("<int>")
               .description("The maximum number of testcases to generate")
               .default_val(16);

auto& trace_opt = Heading::create("Trace options:");
auto& fxn = ValueArg<string>::create("fxn")
            .usage("<string>")
            .description("Function to generate testcases for")
            .default_val("main");
auto& begin_line = ValueArg<size_t>::create("begin_line")
                   .usage("<int>")
                   .description("The line number to begin recording from")
                   .default_val(1);
auto& end_lines = ValueArg<vector<size_t>>::create("end_lines")
                  .usage("{ 0 1 ... 9 }")
                  .description("Line number to end recording on; recording always stops on returns")
                  .default_val({});
auto& max_stack = ValueArg<uint64_t>::create("max_stack")
                  .usage("<bytes>")
                  .description("The maximum number of bytes to assume could be stack")
                  .default_val(1024);

auto& autogen_opt = Heading::create("Autogen options:");
auto& max_attempts = ValueArg<uint64_t>::create("max_attempts")
                     .usage("<int>")
                     .description("The maximum number of attempts to make at generating a testcase")
                     .default_val(16);
auto& max_memory = ValueArg<uint64_t>::create("max_memory")
                   .usage("<bytes>")
                   .description("The maximum number of bytes to allocate to stack or heap")
                   .default_val(1024);
auto& stack_size = ValueArg<size_t>::create("stack_size")
                   .usage("<int>")
                   .description("The minimum stack size available to the testcase")
                   .default_val(16);

auto& conv_opt = Heading::create("File conversion options:");
auto& compress = FlagArg::create("compress")
                 .description("Convert testcase file from text to binary");
auto& decompress = FlagArg::create("decompress")
                   .description("Convert testcase file from binary to text");
auto& in = ValueArg<string>::create("in")
           .alternate("i")
           .usage("<path/to/file.tc>")
           .description("Path to testcases file")
           .default_val("in.tc");

int auto_gen() {
  TargetGadget target;
  SandboxGadget sb({});

  StateGen sg(&sb, stack_size.value());
  sg.set_max_attempts(max_attempts.value())
  .set_max_memory(max_stack.value());

  CpuStates tcs;
  for (size_t i = 0, ie = max_tc.value(); i < ie; ++i) {
    CpuState tc;
    if (sg.get(tc, target)) {
      tcs.push_back(tc);
    }
  }

  if (tcs.empty()) {
    Console::error(1) << "Unable to generate testcases!" << endl;
  }

  if (out.has_been_provided()) {
    ofstream ofs(out.value());
    tcs.write_text(ofs);
  } else {
    tcs.write_text(Console::msg());
    Console::msg() << endl;
  }

  return 0;
}

int trace(const string& argv0) {
  string here = argv0;
  here = here.substr(0, here.find_last_of("/") + 1);

  const string pin_path = here + "../src/ext/pin-2.13-62732-gcc.4.4.7-linux/";
  const string so_path = pin_path + "source/tools/stoke/obj-intel64/";

  Terminal term;
  term << pin_path << "pin -injection child -t " << so_path << "testcase.so ";

  term << "-f " << fxn.value() << " ";
  if (out.has_been_provided()) {
    term << "-o " << out.value() << " ";
  }
  term << "-x " << max_stack.value() << " ";
  term << "-n " << max_tc.value() << " ";
  term << "-b " << begin_line.value() << " ";
  term << "-e \" ";
  for (auto e : end_lines.value()) {
    term << e << " ";
  }
  term << "\" ";

  term << " -- " << bin.value() << " " << args.value() << endl;

  return 0;
}

int do_compress() {
  ifstream ifs(in.value());
  if (!ifs.is_open()) {
    Console::error(1) << "Unable to open input file: " << in.value() << "!" << endl;
  }

  CpuStates cs;
  cs.read_text(ifs);
  if (ifs.fail()) {
    Console::error(1) << "Unable to read input file: " << in.value() << "!" << endl;
  }

  if (out.has_been_provided()) {
    ofstream ofs(out.value());
    cs.write_bin(ofs);
  } else {
    cs.write_bin(Console::msg());
    Console::msg() << endl;
  }

  return 0;
}

int do_decompress() {
  ifstream ifs(in.value());
  if (!ifs.is_open()) {
    Console::error(1) << "Unable to open input file: " << in.value() << "!" << endl;
  }

  CpuStates cs;
  cs.read_bin(ifs);
  if (ifs.fail()) {
    Console::error(1) << "Unable to read input file: " << in.value() << "!" << endl;
  }

  if (out.has_been_provided()) {
    ofstream ofs(out.value());
    cs.write_text(ofs);
  } else {
    cs.write_text(Console::msg());
    Console::msg() << endl;
  }

  return 0;
}

int main(int argc, char** argv) {
  target_arg.required(false);
  CommandLineConfig::strict_with_convenience(argc, argv);
  DebugHandler::install_sigsegv();
  DebugHandler::install_sigill();

  if (compress.value()) {
    return do_compress();
  } else if (decompress.value()) {
    return do_decompress();
  } else if (target_arg.has_been_provided()) {
    return auto_gen();
  } else {
    return trace(argv[0]);
  }
}

<|endoftext|>


<commit_before>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2018 Intel Corporation. All Rights Reserved.

#include <iostream>

#include "librealsense2/rs.hpp"

#include "tclap/CmdLine.h"

#include "converters/converter-csv.hpp"
#include "converters/converter-png.hpp"
#include "converters/converter-raw.hpp"
#include "converters/converter-ply.hpp"
#include "converters/converter-bin.hpp"

#include <mutex>

#define SECONDS_TO_NANOSECONDS 1000000000

using namespace std;
using namespace TCLAP;

int main(int argc, char** argv) try
{
    rs2::log_to_file(RS2_LOG_SEVERITY_WARN);

    // Parse command line arguments
    CmdLine cmd("librealsense rs-convert tool", ' ');
    ValueArg<string> inputFilename("i", "input", "ROS-bag filename", true, "", "ros-bag-file");
    ValueArg<string> outputFilenamePng("p", "output-png", "output PNG file(s) path", false, "", "png-path");
    ValueArg<string> outputFilenameCsv("v", "output-csv", "output CSV (depth matrix) file(s) path", false, "", "csv-path");
    ValueArg<string> outputFilenameRaw("r", "output-raw", "output RAW file(s) path", false, "", "raw-path");
    ValueArg<string> outputFilenamePly("l", "output-ply", "output PLY file(s) path", false, "", "ply-path");
    ValueArg<string> outputFilenameBin("b", "output-bin", "output BIN (depth matrix) file(s) path", false, "", "bin-path");
    SwitchArg switchDepth("d", "depth", "convert depth frames (default - all supported)", false);
    SwitchArg switchColor("c", "color", "convert color frames (default - all supported)", false);
    ValueArg <string> frameNumberStart("f", "first-framenumber", "ignore frames whose frame number is less than this value", false, "", "first-framenumber");
    ValueArg <string> frameNumberEnd("t", "last-framenumber", "ignore frames whose frame number is greater than this value", false, "", "last-framenumber");
    ValueArg <string> startTime("s", "start-time", "ignore frames whose timestamp is less than this value (the first frame is at time 0)", false, "", "start-time");
    ValueArg <string> endTime("e", "end-time", "ignore frames whose timestamp is greater than this value (the first frame is at time 0)", false, "", "end-time");

    cmd.add(inputFilename);
    cmd.add(frameNumberEnd);
    cmd.add(frameNumberStart);
    cmd.add(endTime);
    cmd.add(startTime);
    cmd.add(outputFilenamePng);
    cmd.add(outputFilenameCsv);
    cmd.add(outputFilenameRaw);
    cmd.add(outputFilenamePly);
    cmd.add(outputFilenameBin);
    cmd.add(switchDepth);
    cmd.add(switchColor);
    cmd.parse(argc, argv);

    vector<shared_ptr<rs2::tools::converter::converter_base>> converters;
    shared_ptr<rs2::tools::converter::converter_ply> plyconverter;

    rs2_stream streamType = switchDepth.isSet() ? rs2_stream::RS2_STREAM_DEPTH
        : switchColor.isSet() ? rs2_stream::RS2_STREAM_COLOR
        : rs2_stream::RS2_STREAM_ANY;

    if (outputFilenameCsv.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_csv>(
                outputFilenameCsv.getValue()
                , streamType));
    }

    if (outputFilenamePng.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_png>(
                outputFilenamePng.getValue()
                , streamType));
    }

    if (outputFilenameRaw.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_raw>(
                outputFilenameRaw.getValue()
                , streamType));
    }

    if (outputFilenameBin.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_bin>(
                outputFilenameBin.getValue()));
    }

    if (converters.empty() && !outputFilenamePly.isSet())
    {
        throw runtime_error("output not defined");
    }

    unsigned long long first_frame = 0;
    unsigned long long last_frame = 0;
    uint64_t start_time = 0;
    uint64_t end_time = 0;

    if (frameNumberStart.isSet())
    {
        first_frame = stoi(frameNumberStart.getValue());
    }
    if (frameNumberEnd.isSet())
    {
        last_frame = stoi(frameNumberEnd.getValue());
    }
    if (startTime.isSet())
    {
        start_time = (uint64_t) (SECONDS_TO_NANOSECONDS * (std::strtod( startTime.getValue().c_str(), nullptr )));
    }
    if (endTime.isSet())
    {
        end_time = (uint64_t) (SECONDS_TO_NANOSECONDS * (std::strtod( endTime.getValue().c_str(), nullptr )));
    }

    //in order to convert frames into ply we need synced depth and color frames,
    //therefore we use pipeline
    if (outputFilenamePly.isSet()) {

        // Since we are running in blocking "non-real-time" mode,
        // we don't want to prevent process termination if some of the frames
        // did not find a match and hence were not serviced
        auto pipe = std::shared_ptr<rs2::pipeline>(
            new rs2::pipeline(), [](rs2::pipeline*) {});

        plyconverter = make_shared<rs2::tools::converter::converter_ply>(
            outputFilenamePly.getValue());

        rs2::config cfg;
        cfg.enable_device_from_file(inputFilename.getValue());
        pipe->start(cfg);

        auto device = pipe->get_active_profile().get_device();
        rs2::playback playback = device.as<rs2::playback>();
        playback.set_real_time(false);

        auto duration = playback.get_duration();
        int progress = 0;
        auto frameNumber = 0ULL;

        rs2::frameset frameset;
        uint64_t posCurr = playback.get_position();

        // try_wait_for_frames will keep repeating the last frame at the end of the file,
        // so we need to exit the look in some other way!
        while (pipe->try_wait_for_frames(&frameset, 1000))
        {
            int posP = static_cast<int>(posCurr * 100. / duration.count());
            if (posP > progress)
            {
                progress = posP;
                cout << posP << "%" << "\r" << flush;
            }

            frameNumber = frameset[0].get_frame_number();

            bool process_frame = true;
            if (frameNumberStart.isSet() && frameNumber < first_frame)
                process_frame = false;
            else if (frameNumberEnd.isSet() && frameNumber > last_frame)
                process_frame = false;
            else if (startTime.isSet() && posCurr < start_time)
                process_frame = false;
            else if (endTime.isSet() && posCurr > end_time)
                process_frame = false;

            if( process_frame )
            {
                plyconverter->convert(frameset);
                plyconverter->wait();
            }

            auto posNext = playback.get_position();
            // NOTE: posNext will be 0 if there is no next frame!
            if( posNext <= posCurr )
                break;
            posCurr = posNext;
        }
    }

    // for every converter other than ply,
    // we get the frames from playback sensors
    // and convert them one by one
    if( ! converters.empty() )
    {
        rs2::context ctx;
        auto playback = ctx.load_device(inputFilename.getValue());
        playback.set_real_time(false);
        std::vector<rs2::sensor> sensors = playback.query_sensors();
        std::mutex mutex;

        auto duration = playback.get_duration();
        int progress = 0;
        uint64_t posLast = playback.get_position();

        for (auto sensor : sensors)
        {
            if (!sensor.get_stream_profiles().size())
            {
                continue;
            }

            sensor.open(sensor.get_stream_profiles());
            sensor.start([&](rs2::frame frame)
            {
                std::lock_guard<std::mutex> lock(mutex);

                auto frameNumber = frame.get_frame_number();
                auto frame_time = playback.get_position();

                if (frameNumberStart.isSet() && frameNumber < first_frame)
                    return;
                if (frameNumberEnd.isSet() && frameNumber > last_frame)
                    return;
                if (startTime.isSet() && frame_time < start_time)
                    return;
                if (endTime.isSet() && frame_time > end_time)
                    return;

                for_each(converters.begin(), converters.end(),
                    [&frame](shared_ptr<rs2::tools::converter::converter_base>& converter) {
                    converter->convert(frame);
                });

                for_each(converters.begin(), converters.end(),
                    [](shared_ptr<rs2::tools::converter::converter_base>& converter) {
                    converter->wait();
                });
            });

        }

        //we need to clear the output of ply progress ("100%") before writing
        //the progress of the other converters in the same line
        cout << "\r    \r";

        while (true)
        {
            int posP = static_cast<int>(posLast * 100. / duration.count());

            if (posP > progress)
            {
                progress = posP;
                cout << posP << "%" << "\r" << flush;
            }

            const uint64_t posCurr = playback.get_position();
            if (static_cast<int64_t>(posCurr - posLast) < 0)
            {
                break;
            }
            posLast = posCurr;
        }

        for (auto sensor : sensors)
        {
            if (!sensor.get_stream_profiles().size())
            {
                continue;
            }
            sensor.stop();
            sensor.close();
        }
    }

    cout << endl;

    //print statistics for ply converter.
    if (outputFilenamePly.isSet()) {
        cout << plyconverter->get_statistics() << endl;
    }

    for_each(converters.begin(), converters.end(),
        [](shared_ptr<rs2::tools::converter::converter_base>& converter) {
        cout << converter->get_statistics() << endl;
    });

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    cerr << "RealSense error calling " << e.get_failed_function()
        << "(" << e.get_failed_args() << "):\n    " << e.what() << endl;

    return EXIT_FAILURE;
}
catch (const exception & e)
{
    cerr << e.what() << endl;
    return EXIT_FAILURE;
}
catch (...)
{
    cerr << "some error" << endl;
    return EXIT_FAILURE;
}
<commit_msg>CR fixes<commit_after>// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2018 Intel Corporation. All Rights Reserved.

#include <iostream>

#include "librealsense2/rs.hpp"

#include "tclap/CmdLine.h"

#include "converters/converter-csv.hpp"
#include "converters/converter-png.hpp"
#include "converters/converter-raw.hpp"
#include "converters/converter-ply.hpp"
#include "converters/converter-bin.hpp"

#include <mutex>

#define SECONDS_TO_NANOSECONDS 1000000000

using namespace std;
using namespace TCLAP;

int main(int argc, char** argv) try
{
    rs2::log_to_file(RS2_LOG_SEVERITY_WARN);

    // Parse command line arguments
    CmdLine cmd("librealsense rs-convert tool", ' ');
    ValueArg<string> inputFilename("i", "input", "ROS-bag filename", true, "", "ros-bag-file");
    ValueArg<string> outputFilenamePng("p", "output-png", "output PNG file(s) path", false, "", "png-path");
    ValueArg<string> outputFilenameCsv("v", "output-csv", "output CSV (depth matrix) file(s) path", false, "", "csv-path");
    ValueArg<string> outputFilenameRaw("r", "output-raw", "output RAW file(s) path", false, "", "raw-path");
    ValueArg<string> outputFilenamePly("l", "output-ply", "output PLY file(s) path", false, "", "ply-path");
    ValueArg<string> outputFilenameBin("b", "output-bin", "output BIN (depth matrix) file(s) path", false, "", "bin-path");
    SwitchArg switchDepth("d", "depth", "convert depth frames (default - all supported)", false);
    SwitchArg switchColor("c", "color", "convert color frames (default - all supported)", false);
    ValueArg <string> frameNumberStart("f", "first-framenumber", "ignore frames whose frame number is less than this value", false, "", "first-framenumber");
    ValueArg <string> frameNumberEnd("t", "last-framenumber", "ignore frames whose frame number is greater than this value", false, "", "last-framenumber");
    ValueArg <string> startTime("s", "start-time", "ignore frames whose timestamp is less than this value (the first frame is at time 0)", false, "", "start-time");
    ValueArg <string> endTime("e", "end-time", "ignore frames whose timestamp is greater than this value (the first frame is at time 0)", false, "", "end-time");

    cmd.add(inputFilename);
    cmd.add(frameNumberEnd);
    cmd.add(frameNumberStart);
    cmd.add(endTime);
    cmd.add(startTime);
    cmd.add(outputFilenamePng);
    cmd.add(outputFilenameCsv);
    cmd.add(outputFilenameRaw);
    cmd.add(outputFilenamePly);
    cmd.add(outputFilenameBin);
    cmd.add(switchDepth);
    cmd.add(switchColor);
    cmd.parse(argc, argv);

    vector<shared_ptr<rs2::tools::converter::converter_base>> converters;
    shared_ptr<rs2::tools::converter::converter_ply> plyconverter;

    rs2_stream streamType = switchDepth.isSet() ? rs2_stream::RS2_STREAM_DEPTH
        : switchColor.isSet() ? rs2_stream::RS2_STREAM_COLOR
        : rs2_stream::RS2_STREAM_ANY;

    if (outputFilenameCsv.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_csv>(
                outputFilenameCsv.getValue()
                , streamType));
    }

    if (outputFilenamePng.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_png>(
                outputFilenamePng.getValue()
                , streamType));
    }

    if (outputFilenameRaw.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_raw>(
                outputFilenameRaw.getValue()
                , streamType));
    }

    if (outputFilenameBin.isSet())
    {
        converters.push_back(
            make_shared<rs2::tools::converter::converter_bin>(
                outputFilenameBin.getValue()));
    }

    if (converters.empty() && !outputFilenamePly.isSet())
    {
        throw runtime_error("output not defined");
    }

    unsigned long long first_frame = 0;
    unsigned long long last_frame = 0;
    uint64_t start_time = 0;
    uint64_t end_time = 0;

    if (frameNumberStart.isSet())
    {
        first_frame = stoi(frameNumberStart.getValue());
    }
    if (frameNumberEnd.isSet())
    {
        last_frame = stoi(frameNumberEnd.getValue());
    }
    if (startTime.isSet())
    {
        start_time = (uint64_t) (SECONDS_TO_NANOSECONDS * (std::strtod( startTime.getValue().c_str(), nullptr )));
    }
    if (endTime.isSet())
    {
        end_time = (uint64_t) (SECONDS_TO_NANOSECONDS * (std::strtod( endTime.getValue().c_str(), nullptr )));
    }

    //in order to convert frames into ply we need synced depth and color frames,
    //therefore we use pipeline
    if (outputFilenamePly.isSet()) {

        // Since we are running in blocking "non-real-time" mode,
        // we don't want to prevent process termination if some of the frames
        // did not find a match and hence were not serviced
        auto pipe = std::shared_ptr<rs2::pipeline>(
            new rs2::pipeline(), [](rs2::pipeline*) {});

        plyconverter = make_shared<rs2::tools::converter::converter_ply>(
            outputFilenamePly.getValue());

        rs2::config cfg;
        cfg.enable_device_from_file(inputFilename.getValue());
        pipe->start(cfg);

        auto device = pipe->get_active_profile().get_device();
        rs2::playback playback = device.as<rs2::playback>();
        playback.set_real_time(false);

        auto duration = playback.get_duration();
        int progress = 0;
        auto frameNumber = 0ULL;

        rs2::frameset frameset;
        uint64_t posCurr = playback.get_position();

        // try_wait_for_frames will keep repeating the last frame at the end of the file,
        // so we need to exit the look in some other way!
        while (pipe->try_wait_for_frames(&frameset, 1000))
        {
            int posP = static_cast<int>(posCurr * 100. / duration.count());
            if (posP > progress)
            {
                progress = posP;
                cout << posP << "%" << "\r" << flush;
            }

            frameNumber = frameset[0].get_frame_number();

            bool process_frame = true;
            if (frameNumberStart.isSet() && frameNumber < first_frame)
                process_frame = false;
            else if (frameNumberEnd.isSet() && frameNumber > last_frame)
                process_frame = false;
            else if (startTime.isSet() && posCurr < start_time)
                process_frame = false;
            else if (endTime.isSet() && posCurr > end_time)
                process_frame = false;

            if( process_frame )
            {
                plyconverter->convert(frameset);
                plyconverter->wait();
            }

            auto posNext = playback.get_position();

            // NOTE: posNext will be 0 if there is no next frame!
            if( posNext < posCurr )
                break;
            posCurr = posNext;
        }
    }

    // for every converter other than ply,
    // we get the frames from playback sensors
    // and convert them one by one
    if( ! converters.empty() )
    {
        rs2::context ctx;
        auto playback = ctx.load_device(inputFilename.getValue());
        playback.set_real_time(false);
        std::vector<rs2::sensor> sensors = playback.query_sensors();
        std::mutex mutex;

        auto duration = playback.get_duration();
        int progress = 0;
        uint64_t posCurr = playback.get_position();

        for (auto sensor : sensors)
        {
            if (!sensor.get_stream_profiles().size())
            {
                continue;
            }

            sensor.open(sensor.get_stream_profiles());
            sensor.start([&](rs2::frame frame)
            {
                std::lock_guard<std::mutex> lock(mutex);

                auto frameNumber = frame.get_frame_number();

                if (frameNumberStart.isSet() && frameNumber < first_frame)
                    return;
                if (frameNumberEnd.isSet() && frameNumber > last_frame)
                    return;
                if (startTime.isSet() && posCurr < start_time)
                    return;
                if (endTime.isSet() && posCurr > end_time)
                    return;

                for_each(converters.begin(), converters.end(),
                    [&frame](shared_ptr<rs2::tools::converter::converter_base>& converter) {
                    converter->convert(frame);
                });

                for_each(converters.begin(), converters.end(),
                    [](shared_ptr<rs2::tools::converter::converter_base>& converter) {
                    converter->wait();
                });
            });

        }

        //we need to clear the output of ply progress ("100%") before writing
        //the progress of the other converters in the same line
        cout << "\r    \r";

        while (true)
        {
            int posP = static_cast<int>(posCurr * 100. / duration.count());

            if (posP > progress)
            {
                progress = posP;
                cout << posP << "%" << "\r" << flush;
            }

            const uint64_t posNext = playback.get_position();
            if (posNext < posCurr)
            {
                break;
            }
            posCurr = posNext;
        }

        for (auto sensor : sensors)
        {
            if (!sensor.get_stream_profiles().size())
            {
                continue;
            }
            sensor.stop();
            sensor.close();
        }
    }

    cout << endl;

    //print statistics for ply converter.
    if (outputFilenamePly.isSet()) {
        cout << plyconverter->get_statistics() << endl;
    }

    for_each(converters.begin(), converters.end(),
        [](shared_ptr<rs2::tools::converter::converter_base>& converter) {
        cout << converter->get_statistics() << endl;
    });

    return EXIT_SUCCESS;
}
catch (const rs2::error & e)
{
    cerr << "RealSense error calling " << e.get_failed_function()
        << "(" << e.get_failed_args() << "):\n    " << e.what() << endl;

    return EXIT_FAILURE;
}
catch (const exception & e)
{
    cerr << e.what() << endl;
    return EXIT_FAILURE;
}
catch (...)
{
    cerr << "some error" << endl;
    return EXIT_FAILURE;
}
<|endoftext|>


<commit_before>// fontnik
#include <node_fontnik/glyphs.hpp>

// node
#include <node_buffer.h>
#include <nan.h>

namespace node_fontnik
{

struct RangeBaton {
    v8::Persistent<v8::Function> callback;
    Glyphs *glyphs;
    std::string fontstack;
    std::string range;
    std::vector<std::uint32_t> chars;
    bool error;
    std::string error_name;
};

v8::Persistent<v8::FunctionTemplate> Glyphs::constructor;

Glyphs::Glyphs() : node::ObjectWrap() {
    glyphs = fontnik::Glyphs();
}

Glyphs::Glyphs(const char *data, size_t length) : node::ObjectWrap() {
    glyphs = fontnik::Glyphs(data, length);
}

Glyphs::~Glyphs() {}

void Glyphs::Init(v8::Handle<v8::Object> target) {
    v8::HandleScope scope;

    v8::Local<v8::FunctionTemplate> tpl = NanNew<v8::FunctionTemplate>(New);
    v8::Local<v8::String> name = NanNew<v8::String>("Glyphs");

    constructor = v8::Persistent<v8::FunctionTemplate>::New(tpl);

    // node::ObjectWrap uses the first internal field to store the wrapped pointer.
    constructor->InstanceTemplate()->SetInternalFieldCount(1);
    constructor->SetClassName(name);

    // Add all prototype methods, getters and setters here.
    NODE_SET_PROTOTYPE_METHOD(constructor, "serialize", Serialize);
    NODE_SET_PROTOTYPE_METHOD(constructor, "range", Range);

    // This has to be last, otherwise the properties won't show up on the
    // object in JavaScript.
    target->Set(name, constructor->GetFunction());
}

v8::Handle<v8::Value> Glyphs::New(const v8::Arguments& args) {
    if (!args.IsConstructCall()) {
        return NanThrowTypeError("Constructor must be called with new keyword");
    }
    if (args.Length() > 0 && !node::Buffer::HasInstance(args[0])) {
        return NanThrowTypeError("First argument may only be a buffer");
    }

    Glyphs* glyphs;

    if (args.Length() < 1) {
        glyphs = new Glyphs();
    } else {
        v8::Local<v8::Object> buffer = args[0]->ToObject();
        glyphs = new Glyphs(node::Buffer::Data(buffer), node::Buffer::Length(buffer));
    }

    glyphs->Wrap(args.This());

    return args.This();
}

bool Glyphs::HasInstance(v8::Handle<v8::Value> val) {
    if (!val->IsObject()) return false;
    return constructor->HasInstance(val->ToObject());
}

v8::Handle<v8::Value> Glyphs::Serialize(const v8::Arguments& args) {
    v8::HandleScope scope;
    std::string serialized = node::ObjectWrap::Unwrap<Glyphs>(args.This())->glyphs.Serialize();
    return scope.Close(node::Buffer::New(serialized.data(), serialized.length())->handle_);
}

v8::Handle<v8::Value> Glyphs::Range(const v8::Arguments& args) {
    v8::HandleScope scope;

    // Validate arguments.
    if (args.Length() < 1 || !args[0]->IsString()) {
        return NanThrowTypeError("fontstack must be a string");
    }

    if (args.Length() < 2 || !args[1]->IsString()) {
        return NanThrowTypeError("range must be a string");
    }

    if (args.Length() < 3 || !args[2]->IsArray()) {
        return NanThrowTypeError("chars must be an array");
    }

    if (args.Length() < 4 || !args[3]->IsFunction()) {
        return NanThrowTypeError("callback must be a function");
    }

    v8::String::Utf8Value fontstack(args[0]->ToString());
    v8::String::Utf8Value range(args[1]->ToString());
    v8::Local<v8::Array> charsArray = v8::Local<v8::Array>::Cast(args[2]);
    v8::Local<v8::Function> callback = v8::Local<v8::Function>::Cast(args[3]);

    unsigned array_size = charsArray->Length();
    std::vector<std::uint32_t> chars;
    for (unsigned i=0; i < array_size; i++) {
        chars.push_back(charsArray->Get(i)->IntegerValue());
    }

    Glyphs *glyphs = node::ObjectWrap::Unwrap<Glyphs>(args.This());

    RangeBaton* baton = new RangeBaton();
    baton->callback = v8::Persistent<v8::Function>::New(callback);
    baton->glyphs = glyphs;
    baton->fontstack = *fontstack;
    baton->range = *range;
    baton->chars = chars;

    uv_work_t *req = new uv_work_t();
    req->data = baton;

    int status = uv_queue_work(uv_default_loop(), req, AsyncRange, (uv_after_work_cb)RangeAfter);
    assert(status == 0);

    NanReturnUndefined();
}

void Glyphs::AsyncRange(uv_work_t* req) {
    RangeBaton* baton = static_cast<RangeBaton*>(req->data);

    try {
        baton->glyphs->glyphs.Range(baton->fontstack, baton->range, baton->chars);
    } catch(const std::runtime_error &e) {
        baton->error = true;
        baton->error_name = e.what();
        return;
    }
}

void Glyphs::RangeAfter(uv_work_t* req) {
    v8::HandleScope scope;
    RangeBaton* baton = static_cast<RangeBaton*>(req->data);

    const unsigned argc = 1;

    v8::TryCatch try_catch;

    if (baton->error) {
        v8::Local<v8::Value> argv[argc] = { v8::Exception::Error(v8::String::New(baton->error_name.c_str())) };
        baton->callback->Call(v8::Context::GetCurrent()->Global(), argc, argv);
    } else {
        v8::Local<v8::Value> argv[argc] = { v8::Local<v8::Value>::New(v8::Null()) };
        baton->callback->Call(v8::Context::GetCurrent()->Global(), argc, argv);
    }

    if (try_catch.HasCaught()) {
        node::FatalException(try_catch);
    }

    baton->callback.Dispose();

    delete baton;
    delete req;
}

} // ns node_fontnik
<commit_msg>More NaN<commit_after>// fontnik
#include <node_fontnik/glyphs.hpp>

// node
#include <node_buffer.h>
#include <nan.h>

namespace node_fontnik
{

struct RangeBaton {
    v8::Persistent<v8::Function> callback;
    Glyphs *glyphs;
    std::string fontstack;
    std::string range;
    std::vector<std::uint32_t> chars;
    bool error;
    std::string error_name;
};

v8::Persistent<v8::FunctionTemplate> Glyphs::constructor;

Glyphs::Glyphs() : node::ObjectWrap() {
    glyphs = fontnik::Glyphs();
}

Glyphs::Glyphs(const char *data, size_t length) : node::ObjectWrap() {
    glyphs = fontnik::Glyphs(data, length);
}

Glyphs::~Glyphs() {}

void Glyphs::Init(v8::Handle<v8::Object> target) {
    NanScope();

    v8::Local<v8::FunctionTemplate> tpl = NanNew<v8::FunctionTemplate>(New);
    v8::Local<v8::String> name = NanNew<v8::String>("Glyphs");

    constructor = v8::Persistent<v8::FunctionTemplate>::New(tpl);

    // node::ObjectWrap uses the first internal field to store the wrapped pointer.
    constructor->InstanceTemplate()->SetInternalFieldCount(1);
    constructor->SetClassName(name);

    // Add all prototype methods, getters and setters here.
    NODE_SET_PROTOTYPE_METHOD(constructor, "serialize", Serialize);
    NODE_SET_PROTOTYPE_METHOD(constructor, "range", Range);

    // This has to be last, otherwise the properties won't show up on the
    // object in JavaScript.
    target->Set(name, constructor->GetFunction());
}

v8::Handle<v8::Value> Glyphs::New(const v8::Arguments& args) {
    if (!args.IsConstructCall()) {
        return NanThrowTypeError("Constructor must be called with new keyword");
    }
    if (args.Length() > 0 && !node::Buffer::HasInstance(args[0])) {
        return NanThrowTypeError("First argument may only be a buffer");
    }

    Glyphs* glyphs;

    if (args.Length() < 1) {
        glyphs = new Glyphs();
    } else {
        v8::Local<v8::Object> buffer = args[0]->ToObject();
        glyphs = new Glyphs(node::Buffer::Data(buffer), node::Buffer::Length(buffer));
    }

    glyphs->Wrap(args.This());

    return args.This();
}

bool Glyphs::HasInstance(v8::Handle<v8::Value> val) {
    if (!val->IsObject()) return false;
    return constructor->HasInstance(val->ToObject());
}

NAN_METHOD(Glyphs::Serialize) {
    NanScope();
    std::string serialized = node::ObjectWrap::Unwrap<Glyphs>(args.This())->glyphs.Serialize();
    return scope.Close(node::Buffer::New(serialized.data(), serialized.length())->handle_);
}

NAN_METHOD(Glyphs::Range) {
    NanScope();

    // Validate arguments.
    if (args.Length() < 1 || !args[0]->IsString()) {
        return NanThrowTypeError("fontstack must be a string");
    }

    if (args.Length() < 2 || !args[1]->IsString()) {
        return NanThrowTypeError("range must be a string");
    }

    if (args.Length() < 3 || !args[2]->IsArray()) {
        return NanThrowTypeError("chars must be an array");
    }

    if (args.Length() < 4 || !args[3]->IsFunction()) {
        return NanThrowTypeError("callback must be a function");
    }

    v8::String::Utf8Value fontstack(args[0]->ToString());
    v8::String::Utf8Value range(args[1]->ToString());
    v8::Local<v8::Array> charsArray = v8::Local<v8::Array>::Cast(args[2]);
    v8::Local<v8::Function> callback = v8::Local<v8::Function>::Cast(args[3]);

    unsigned array_size = charsArray->Length();
    std::vector<std::uint32_t> chars;
    for (unsigned i=0; i < array_size; i++) {
        chars.push_back(charsArray->Get(i)->IntegerValue());
    }

    Glyphs *glyphs = node::ObjectWrap::Unwrap<Glyphs>(args.This());

    RangeBaton* baton = new RangeBaton();
    baton->callback = v8::Persistent<v8::Function>::New(callback);
    baton->glyphs = glyphs;
    baton->fontstack = *fontstack;
    baton->range = *range;
    baton->chars = chars;

    uv_work_t *req = new uv_work_t();
    req->data = baton;

    int status = uv_queue_work(uv_default_loop(), req, AsyncRange, (uv_after_work_cb)RangeAfter);
    assert(status == 0);

    NanReturnUndefined();
}

void Glyphs::AsyncRange(uv_work_t* req) {
    RangeBaton* baton = static_cast<RangeBaton*>(req->data);

    try {
        baton->glyphs->glyphs.Range(baton->fontstack, baton->range, baton->chars);
    } catch(const std::runtime_error &e) {
        baton->error = true;
        baton->error_name = e.what();
        return;
    }
}

void Glyphs::RangeAfter(uv_work_t* req) {
    NanScope();
    RangeBaton* baton = static_cast<RangeBaton*>(req->data);

    const unsigned argc = 1;

    v8::TryCatch try_catch;
    v8::Local<v8::Context> ctx = NanGetCurrentContext();

    if (baton->error) {
        v8::Local<v8::Value> argv[argc] = { v8::Exception::Error(NanNew<v8::String>(baton->error_name.c_str())) };
        baton->callback->Call(ctx->Global(), argc, argv);
    } else {
        v8::Local<v8::Value> argv[argc] = { v8::Local<v8::Value>::New(NanNull()) };
        baton->callback->Call(ctx->Global(), argc, argv);
    }

    if (try_catch.HasCaught()) {
        node::FatalException(try_catch);
    }

    baton->callback.Dispose();

    delete baton;
    delete req;
}

} // ns node_fontnik
<|endoftext|>


<commit_before>
#include "function.h"
#include "value.h"
#include "closure.h"
#include "gi.h"
#include "gobject.h"
#include "debug.h"

using namespace v8;

namespace GNodeJS {

static G_DEFINE_QUARK(gnode_js_object, gnode_js_object);
static G_DEFINE_QUARK(gnode_js_template, gnode_js_template);

static bool InitGParameterFromProperty(GParameter    *parameter,
                                       void          *klass,
                                       Handle<String> name,
                                       Handle<Value>  value) {
    String::Utf8Value name_str (name);
    GParamSpec *pspec = g_object_class_find_property (G_OBJECT_CLASS (klass), *name_str);
    if (pspec == NULL)
        return false;

    parameter->name = pspec->name;
    g_value_init (&parameter->value, G_PARAM_SPEC_VALUE_TYPE (pspec));
    V8ToGValue (&parameter->value, value);
    return true;
}

static bool InitGParametersFromProperty(GParameter    **parameters_p,
                                        int            *n_parameters_p,
                                        void           *klass,
                                        Handle<Object>  property_hash) {
    Local<Array> properties = property_hash->GetOwnPropertyNames ();
    int n_parameters = properties->Length ();
    GParameter *parameters = g_new0 (GParameter, n_parameters);

    for (int i = 0; i < n_parameters; i++) {
        Local<Value> name = properties->Get (i);
        Local<Value> value = property_hash->Get (name);

        if (!InitGParameterFromProperty (&parameters[i], klass, name->ToString (), value))
            return false;
    }

    *parameters_p = parameters;
    *n_parameters_p = n_parameters;
    return true;
}

static void ToggleNotify(gpointer user_data, GObject *gobject, gboolean toggle_down);

static void AssociateGObject(Isolate *isolate, Handle<Object> object, GObject *gobject) {
    object->SetAlignedPointerInInternalField (0, gobject);

    g_object_ref_sink (gobject);
    g_object_add_toggle_ref (gobject, ToggleNotify, NULL);

    Persistent<Object> *persistent = new Persistent<Object>(isolate, object);
    g_object_set_qdata (gobject, gnode_js_object_quark (), persistent);
}

static void GObjectConstructor(const FunctionCallbackInfo<Value> &args) {
    Isolate *isolate = args.GetIsolate ();

    /* The flow of this function is a bit twisty.

     * There's two cases for when this code is called:
     * user code doing `new Gtk.Widget({ ... })`, and
     * internal code as part of WrapperFromGObject, where
     * the constructor is called with one external. */

    if (!args.IsConstructCall ()) {
        THROW(Exception::TypeError, "Not a construct call.");
        return;
    }

    Handle<Object> self = args.This ();

    if (args[0]->IsExternal ()) {
        /* The External case. This is how WrapperFromGObject is called. */

        void *data = External::Cast (*args[0])->Value ();
        GObject *gobject = G_OBJECT (data);

        AssociateGObject (isolate, self, gobject);
    } else {
        /* User code calling `new Gtk.Widget({ ... })` */

        GObject *gobject;
        GIBaseInfo *info = (GIBaseInfo *) External::Cast (*args.Data ())->Value ();
        GType gtype = g_registered_type_info_get_g_type ((GIRegisteredTypeInfo *) info);
        void *klass = g_type_class_ref (gtype);

        GParameter *parameters = NULL;
        int n_parameters = 0;

        if (args[0]->IsObject ()) {
            Local<Object> property_hash = args[0]->ToObject ();

            if (!InitGParametersFromProperty (&parameters, &n_parameters, klass, property_hash)) {
                THROW(Exception::TypeError, "Unable to make GParameters.");
                goto out;
            }
        }

        gobject = (GObject *) g_object_newv (gtype, n_parameters, parameters);
        AssociateGObject (isolate, self, gobject);

    out:
        g_free (parameters);
        g_type_class_unref (klass);
    }
}

static void SignalConnectInternal(const FunctionCallbackInfo<Value> &args, bool after) {
    Isolate *isolate = args.GetIsolate ();
    GObject *gobject = GObjectFromWrapper (args.This ());

    String::Utf8Value signal_name (args[0]->ToString ());
    Handle<Function> callback = Local<Function>::Cast (args[1]->ToObject ());
    GClosure *gclosure = MakeClosure (isolate, callback);

    ulong handler_id = g_signal_connect_closure (gobject, *signal_name, gclosure, after);
    args.GetReturnValue ().Set(Integer::NewFromUnsigned (isolate, handler_id));
}

static void SignalConnect(const FunctionCallbackInfo<Value> &args) {
    SignalConnectInternal (args, false);
}

static Handle<FunctionTemplate> GetBaseClassTemplate(Isolate *isolate) {
    Local<FunctionTemplate> tpl = FunctionTemplate::New (isolate);
    Handle<ObjectTemplate> proto = tpl->PrototypeTemplate ();
    proto->Set (String::NewFromUtf8 (isolate, "connect"), FunctionTemplate::New (isolate, SignalConnect)->GetFunction ());
    return tpl;
}

static Handle<FunctionTemplate> GetClassTemplateFromGI(Isolate *isolate, GIBaseInfo *info);

static void ClassDestroyed(const WeakCallbackData<FunctionTemplate, GIBaseInfo> &data) {
    GIBaseInfo *info = data.GetParameter ();
    GType gtype = g_registered_type_info_get_g_type ((GIRegisteredTypeInfo *) info);

    void *type_data = g_type_get_qdata (gtype, gnode_js_template_quark ());
    assert (type_data != NULL);
    Persistent<FunctionTemplate> *persistent = (Persistent<FunctionTemplate> *) type_data;
    delete persistent;

    g_type_set_qdata (gtype, gnode_js_template_quark (), NULL);
    g_base_info_unref (info);
}

static Handle<FunctionTemplate> GetClassTemplate(Isolate *isolate, GIBaseInfo *info, GType gtype) {
    void *data = g_type_get_qdata (gtype, gnode_js_template_quark ());

    if (data) {
        Persistent<FunctionTemplate> *persistent = (Persistent<FunctionTemplate> *) data;
        Handle<FunctionTemplate> tpl = Handle<FunctionTemplate>::New (isolate, *persistent);
        return tpl;
    } else {
        //printf("\x1b[38;5;201mGetClassTemplate \x1b[93m%lu\x1b[0m %s \n", gtype, g_type_name(gtype));
        //const char *class_name = g_base_info_get_name (info);
        const char *class_name = g_type_name (gtype);

        Handle<FunctionTemplate> tpl = FunctionTemplate::New (isolate, GObjectConstructor, External::New (isolate, info));
        tpl->SetClassName (String::NewFromUtf8 (isolate, class_name));
        tpl->InstanceTemplate ()->SetInternalFieldCount (1);

        Persistent<FunctionTemplate> *persistent = new Persistent<FunctionTemplate>(isolate, tpl);
        persistent->SetWeak (g_base_info_ref (info), ClassDestroyed);
        g_type_set_qdata (gtype, gnode_js_template_quark (), persistent);

        GIObjectInfo *parent_info = g_object_info_get_parent (info);
        if (parent_info) {
            Handle<FunctionTemplate> parent_tpl = GetClassTemplateFromGI (isolate, (GIBaseInfo *) parent_info);
            tpl->Inherit (parent_tpl);
        } else {
            tpl->Inherit (GetBaseClassTemplate (isolate));
        }

        return tpl;
    }
}

static Handle<FunctionTemplate> GetClassTemplateFromGI(Isolate *isolate, GIBaseInfo *info) {
    GType gtype = g_registered_type_info_get_g_type ((GIRegisteredTypeInfo *) info);
    return GetClassTemplate (isolate, info, gtype);
}

static Handle<FunctionTemplate> GetClassTemplateFromGType(Isolate *isolate, GType gtype) {
    GIRepository *repo = g_irepository_get_default ();
    GIBaseInfo *info = g_irepository_find_by_gtype (repo, gtype);
    while (info == NULL) {
        gtype = g_type_parent(gtype);
        info = g_irepository_find_by_gtype (repo, gtype);
    }
    return GetClassTemplate (isolate, info, gtype);
}

Handle<Function> MakeClass(Isolate *isolate, GIBaseInfo *info) {
    Handle<FunctionTemplate> tpl = GetClassTemplateFromGI (isolate, info);
    return tpl->GetFunction ();
}

static void ObjectDestroyed(const WeakCallbackData<Object, GObject> &data) {
    GObject *gobject = data.GetParameter ();

    void *type_data = g_object_get_qdata (gobject, gnode_js_object_quark ());
    assert (type_data != NULL);
    Persistent<Object> *persistent = (Persistent<Object> *) type_data;
    delete persistent;

    /* We're destroying the wrapper object, so make sure to clear out
     * the qdata that points back to us. */
    g_object_set_qdata (gobject, gnode_js_object_quark (), NULL);

    g_object_unref (gobject);
}

static void ToggleNotify(gpointer user_data, GObject *gobject, gboolean toggle_down) {
    void *data = g_object_get_qdata (gobject, gnode_js_object_quark ());
    assert (data != NULL);

    Persistent<Object> *persistent = (Persistent<Object> *) data;

    if (toggle_down) {
        /* We're dropping from 2 refs to 1 ref. We are the last holder. Make
         * sure that that our weak ref is installed. */
        persistent->SetWeak (gobject, ObjectDestroyed);
    } else {
        /* We're going from 1 ref to 2 refs. We can't let our wrapper be
         * collected, so make sure that our reference is persistent */
        persistent->ClearWeak ();
    }
}

Handle<Value> WrapperFromGObject(Isolate *isolate, GIBaseInfo *info, GObject *gobject) {
    void *data = g_object_get_qdata (gobject, gnode_js_object_quark ());

    if (data) {
        /* Easy case: we already have an object. */
        Persistent<Object> *persistent = (Persistent<Object> *) data;
        Handle<Object> obj = Handle<Object>::New (isolate, *persistent);
        return obj;
    } else {
        GType        type = G_OBJECT_TYPE(gobject);
        const char *name  = G_OBJECT_TYPE_NAME(gobject);
        //GTypePlugin *plugin = g_type_get_plugin(type);
        void *klass = g_type_class_ref (type);

        DEBUG("GObject: %s \n", name);
        printf("\x1b[38;5;202mclass: %s \x1b[0;93m %lu \x1b[0m\n",
                G_OBJECT_CLASS_NAME (klass),
                G_OBJECT_CLASS_TYPE (klass) );
        print_type(type);

        if (info != NULL) {
            print_info(info);
            //if (GI_IS_OBJECT_INFO(info)) {
                //GIBaseInfo *class_info = g_object_info_get_class_struct(info);
                //print_info(class_info);
                //g_base_info_unref(class_info);
            //}
        }

        Handle<FunctionTemplate> tpl;

        tpl = GetClassTemplateFromGType (isolate, type);

        Handle<Function> constructor = tpl->GetFunction ();
        Handle<Value> gobject_external = External::New (isolate, gobject);
        Handle<Value> args[] = { gobject_external };
        Handle<Object> obj = constructor->NewInstance (1, args);

        //g_base_info_unref(info);
        g_type_class_unref (klass);

        return obj;
    }
}

GObject * GObjectFromWrapper(Handle<Value> value) {
    Handle<Object> object = value->ToObject ();
    void *data = object->GetAlignedPointerFromInternalField (0);
    GObject *gobject = G_OBJECT (data);
    return gobject;
}

};
<commit_msg><commit_after>
#include "function.h"
#include "value.h"
#include "closure.h"
#include "gi.h"
#include "gobject.h"
#include "debug.h"

using namespace v8;

namespace GNodeJS {

static G_DEFINE_QUARK(gnode_js_object, gnode_js_object);
static G_DEFINE_QUARK(gnode_js_template, gnode_js_template);

static bool InitGParameterFromProperty(GParameter    *parameter,
                                       void          *klass,
                                       Handle<String> name,
                                       Handle<Value>  value) {
    String::Utf8Value name_str (name);
    GParamSpec *pspec = g_object_class_find_property (G_OBJECT_CLASS (klass), *name_str);
    if (pspec == NULL)
        return false;

    parameter->name = pspec->name;
    g_value_init (&parameter->value, G_PARAM_SPEC_VALUE_TYPE (pspec));
    V8ToGValue (&parameter->value, value);
    return true;
}

static bool InitGParametersFromProperty(GParameter    **parameters_p,
                                        int            *n_parameters_p,
                                        void           *klass,
                                        Handle<Object>  property_hash) {
    Local<Array> properties = property_hash->GetOwnPropertyNames ();
    int n_parameters = properties->Length ();
    GParameter *parameters = g_new0 (GParameter, n_parameters);

    for (int i = 0; i < n_parameters; i++) {
        Local<Value> name = properties->Get (i);
        Local<Value> value = property_hash->Get (name);

        if (!InitGParameterFromProperty (&parameters[i], klass, name->ToString (), value))
            return false;
    }

    *parameters_p = parameters;
    *n_parameters_p = n_parameters;
    return true;
}

static void ToggleNotify(gpointer user_data, GObject *gobject, gboolean toggle_down);

static void AssociateGObject(Isolate *isolate, Handle<Object> object, GObject *gobject) {
    object->SetAlignedPointerInInternalField (0, gobject);

    g_object_ref_sink (gobject);
    g_object_add_toggle_ref (gobject, ToggleNotify, NULL);

    Persistent<Object> *persistent = new Persistent<Object>(isolate, object);
    g_object_set_qdata (gobject, gnode_js_object_quark (), persistent);
}

static void GObjectConstructor(const FunctionCallbackInfo<Value> &args) {
    Isolate *isolate = args.GetIsolate ();

    /* The flow of this function is a bit twisty.

     * There's two cases for when this code is called:
     * user code doing `new Gtk.Widget({ ... })`, and
     * internal code as part of WrapperFromGObject, where
     * the constructor is called with one external. */

    if (!args.IsConstructCall ()) {
        THROW(Exception::TypeError, "Not a construct call.");
        return;
    }

    Handle<Object> self = args.This ();

    if (args[0]->IsExternal ()) {
        /* The External case. This is how WrapperFromGObject is called. */

        void *data = External::Cast (*args[0])->Value ();
        GObject *gobject = G_OBJECT (data);

        AssociateGObject (isolate, self, gobject);
    } else {
        /* User code calling `new Gtk.Widget({ ... })` */

        GObject *gobject;
        GIBaseInfo *info = (GIBaseInfo *) External::Cast (*args.Data ())->Value ();
        GType gtype = g_registered_type_info_get_g_type ((GIRegisteredTypeInfo *) info);
        void *klass = g_type_class_ref (gtype);

        GParameter *parameters = NULL;
        int n_parameters = 0;

        if (args[0]->IsObject ()) {
            Local<Object> property_hash = args[0]->ToObject ();

            if (!InitGParametersFromProperty (&parameters, &n_parameters, klass, property_hash)) {
                THROW(Exception::TypeError, "Unable to make GParameters.");
                goto out;
            }
        }

        gobject = (GObject *) g_object_newv (gtype, n_parameters, parameters);
        AssociateGObject (isolate, self, gobject);

    out:
        g_free (parameters);
        g_type_class_unref (klass);
    }
}

static void SignalConnectInternal(const Nan::FunctionCallbackInfo<v8::Value> &args, bool after) {
    Isolate *isolate = args.GetIsolate ();
    GObject *gobject = GObjectFromWrapper (args.This ());

    String::Utf8Value signal_name (args[0]->ToString ());
    Handle<Function> callback = Local<Function>::Cast (args[1]->ToObject ());
    GClosure *gclosure = MakeClosure (isolate, callback);

    ulong handler_id = g_signal_connect_closure (gobject, *signal_name, gclosure, after);
    args.GetReturnValue ().Set(Integer::NewFromUnsigned (isolate, handler_id));
}

NAN_METHOD(SignalConnect) {
    SignalConnectInternal(info, false);
}

static Handle<FunctionTemplate> GetBaseClassTemplate(Isolate *isolate) {
    Local<FunctionTemplate> tpl = FunctionTemplate::New (isolate);
    Nan::SetPrototypeMethod(tpl, "on", SignalConnect);
    Nan::SetPrototypeMethod(tpl, "connect", SignalConnect);
    Nan::SetPrototypeMethod(tpl, "addEventListener", SignalConnect);
    return tpl;
}

static Handle<FunctionTemplate> GetClassTemplateFromGI(Isolate *isolate, GIBaseInfo *info);

static void ClassDestroyed(const WeakCallbackData<FunctionTemplate, GIBaseInfo> &data) {
    GIBaseInfo *info = data.GetParameter ();
    GType gtype = g_registered_type_info_get_g_type ((GIRegisteredTypeInfo *) info);

    void *type_data = g_type_get_qdata (gtype, gnode_js_template_quark ());
    assert (type_data != NULL);
    Persistent<FunctionTemplate> *persistent = (Persistent<FunctionTemplate> *) type_data;
    delete persistent;

    g_type_set_qdata (gtype, gnode_js_template_quark (), NULL);
    g_base_info_unref (info);
}

static Handle<FunctionTemplate> GetClassTemplate(Isolate *isolate, GIBaseInfo *info, GType gtype) {
    void *data = g_type_get_qdata (gtype, gnode_js_template_quark ());

    if (data) {
        Persistent<FunctionTemplate> *persistent = (Persistent<FunctionTemplate> *) data;
        Handle<FunctionTemplate> tpl = Handle<FunctionTemplate>::New (isolate, *persistent);
        return tpl;
    } else {
        //printf("\x1b[38;5;201mGetClassTemplate \x1b[93m%lu\x1b[0m %s \n", gtype, g_type_name(gtype));
        //const char *class_name = g_base_info_get_name (info);
        const char *class_name = g_type_name (gtype);

        Handle<FunctionTemplate> tpl = FunctionTemplate::New (isolate, GObjectConstructor, External::New (isolate, info));
        tpl->SetClassName (String::NewFromUtf8 (isolate, class_name));
        tpl->InstanceTemplate ()->SetInternalFieldCount (1);

        Persistent<FunctionTemplate> *persistent = new Persistent<FunctionTemplate>(isolate, tpl);
        persistent->SetWeak (g_base_info_ref (info), ClassDestroyed);
        g_type_set_qdata (gtype, gnode_js_template_quark (), persistent);

        GIObjectInfo *parent_info = g_object_info_get_parent (info);
        if (parent_info) {
            Handle<FunctionTemplate> parent_tpl = GetClassTemplateFromGI (isolate, (GIBaseInfo *) parent_info);
            tpl->Inherit (parent_tpl);
        } else {
            tpl->Inherit (GetBaseClassTemplate (isolate));
        }

        return tpl;
    }
}

static Handle<FunctionTemplate> GetClassTemplateFromGI(Isolate *isolate, GIBaseInfo *info) {
    GType gtype = g_registered_type_info_get_g_type ((GIRegisteredTypeInfo *) info);
    return GetClassTemplate (isolate, info, gtype);
}

static Handle<FunctionTemplate> GetClassTemplateFromGType(Isolate *isolate, GType gtype) {
    GIRepository *repo = g_irepository_get_default ();
    GIBaseInfo *info = g_irepository_find_by_gtype (repo, gtype);
    while (info == NULL) {
        gtype = g_type_parent(gtype);
        info = g_irepository_find_by_gtype (repo, gtype);
    }
    return GetClassTemplate (isolate, info, gtype);
}

Handle<Function> MakeClass(Isolate *isolate, GIBaseInfo *info) {
    Handle<FunctionTemplate> tpl = GetClassTemplateFromGI (isolate, info);
    return tpl->GetFunction ();
}

static void ObjectDestroyed(const WeakCallbackData<Object, GObject> &data) {
    GObject *gobject = data.GetParameter ();

    void *type_data = g_object_get_qdata (gobject, gnode_js_object_quark ());
    assert (type_data != NULL);
    Persistent<Object> *persistent = (Persistent<Object> *) type_data;
    delete persistent;

    /* We're destroying the wrapper object, so make sure to clear out
     * the qdata that points back to us. */
    g_object_set_qdata (gobject, gnode_js_object_quark (), NULL);

    g_object_unref (gobject);
}

static void ToggleNotify(gpointer user_data, GObject *gobject, gboolean toggle_down) {
    void *data = g_object_get_qdata (gobject, gnode_js_object_quark ());
    assert (data != NULL);

    Persistent<Object> *persistent = (Persistent<Object> *) data;

    if (toggle_down) {
        /* We're dropping from 2 refs to 1 ref. We are the last holder. Make
         * sure that that our weak ref is installed. */
        persistent->SetWeak (gobject, ObjectDestroyed);
    } else {
        /* We're going from 1 ref to 2 refs. We can't let our wrapper be
         * collected, so make sure that our reference is persistent */
        persistent->ClearWeak ();
    }
}

Handle<Value> WrapperFromGObject(Isolate *isolate, GIBaseInfo *info, GObject *gobject) {
    void *data = g_object_get_qdata (gobject, gnode_js_object_quark ());

    if (data) {
        /* Easy case: we already have an object. */
        Persistent<Object> *persistent = (Persistent<Object> *) data;
        Handle<Object> obj = Handle<Object>::New (isolate, *persistent);
        return obj;
    } else {
        GType        type = G_OBJECT_TYPE(gobject);
        const char *name  = G_OBJECT_TYPE_NAME(gobject);
        //GTypePlugin *plugin = g_type_get_plugin(type);
        void *klass = g_type_class_ref (type);

        DEBUG("GObject: %s \n", name);
        printf("\x1b[38;5;202mclass: %s \x1b[0;93m %lu \x1b[0m\n",
                G_OBJECT_CLASS_NAME (klass),
                G_OBJECT_CLASS_TYPE (klass) );
        print_type(type);

        if (info != NULL) {
            print_info(info);
            //if (GI_IS_OBJECT_INFO(info)) {
                //GIBaseInfo *class_info = g_object_info_get_class_struct(info);
                //print_info(class_info);
                //g_base_info_unref(class_info);
            //}
        }

        Handle<FunctionTemplate> tpl;

        tpl = GetClassTemplateFromGType (isolate, type);

        Handle<Function> constructor = tpl->GetFunction ();
        Handle<Value> gobject_external = External::New (isolate, gobject);
        Handle<Value> args[] = { gobject_external };
        Handle<Object> obj = constructor->NewInstance (1, args);

        //g_base_info_unref(info);
        g_type_class_unref (klass);

        return obj;
    }
}

GObject * GObjectFromWrapper(Handle<Value> value) {
    Handle<Object> object = value->ToObject ();
    void *data = object->GetAlignedPointerFromInternalField (0);
    GObject *gobject = G_OBJECT (data);
    return gobject;
}

};
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 The Apollo 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.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::PathPoint;
using apollo::common::math::Vec2d;
using apollo::common::VehicleConfigHelper;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

Stage::StageStatus SidePassStopOnWaitPoint::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  ADEBUG << "Processing SidePassStopOnWaitPoint";
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  const PathDecision& path_decision = reference_line_info.path_decision();

  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty.";
    return Stage::ERROR;
  }

  if (!GetContext()->path_data_.UpdateFrenetFramePath(&reference_line)) {
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line.GetFrenetPoint(frame->PlanningStartPoint().path_point());

  if (!GetContext()->path_data_.LeftTrimWithRefS(adc_frenet_frame_point_)) {
    return Stage::ERROR;
  }
  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty after trim.";
    return Stage::ERROR;
  }

  for (const auto& p :
       GetContext()->path_data_.discretized_path().path_points()) {
    ADEBUG << p.ShortDebugString();
  }

  // Get the nearest obstacle
  const Obstacle* nearest_obstacle = nullptr;
  if (!GetTheNearestObstacle(reference_line, path_decision.obstacles(),
                             &nearest_obstacle)) {
    AERROR << "Failed while running the function to get nearest obstacle.";
    return Stage::ERROR;
  }

  // If the nearest obstacle, provided it exists, is moving,
  // then quit the side_pass stage.
  if (nearest_obstacle) {
    if (nearest_obstacle->speed() >
        GetContext()->scenario_config_.block_obstacle_min_speed()) {
      ADEBUG << "The nearest obstacle to side-pass is moving.";
      next_stage_ = ScenarioConfig::NO_STAGE;
      return Stage::FINISHED;
    }
  }

  ADEBUG << "Got the nearest obstacle if there is one.";
  // Get the "wait point".
  PathPoint first_path_point =
      GetContext()->path_data_.discretized_path().path_points().front();
  PathPoint last_path_point;
  bool should_not_move_at_all = false;
  if (!GetMoveForwardLastPathPoint(reference_line, nearest_obstacle,
                                   &last_path_point, &should_not_move_at_all)) {
    ADEBUG << "Fail to get move forward last path point.";
    return Stage::ERROR;
  }
  if (should_not_move_at_all) {
    ADEBUG << "The ADC is already at a stop point.";
    next_stage_ = ScenarioConfig::SIDE_PASS_DETECT_SAFETY;
    return Stage::FINISHED;  // return FINISHED if it's already at "wait point".
  }

  ADEBUG << "first_path_point: " << first_path_point.ShortDebugString();
  ADEBUG << "last_path_point : " << last_path_point.ShortDebugString();
  double move_forward_distance = last_path_point.s() - first_path_point.s();
  ADEBUG << "move_forward_distance: " << move_forward_distance;

  // Wait until everything is clear.
  if (!IsFarAwayFromObstacles(reference_line, path_decision.obstacles(),
                              first_path_point, last_path_point)) {
    // wait here, do nothing this cycle.
    auto& rfl_info = frame->mutable_reference_line_info()->front();
    *(rfl_info.mutable_path_data()) = GetContext()->path_data_;
    *(rfl_info.mutable_speed_data()) =
        SpeedProfileGenerator::GenerateFallbackSpeedProfile();

    rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
    DiscretizedTrajectory trajectory;
    if (!rfl_info.CombinePathAndSpeedProfile(
            frame->PlanningStartPoint().relative_time(),
            frame->PlanningStartPoint().path_point().s(), &trajectory)) {
      AERROR << "Fail to aggregate planning trajectory.";
      return Stage::RUNNING;
    }
    rfl_info.SetTrajectory(trajectory);
    rfl_info.SetDrivable(true);

    ADEBUG << "waiting until obstacles are far away.";
    return Stage::RUNNING;
  }

  // (1) call proceed with cautious
  constexpr double kSidePassCreepSpeed = 2.33;  // m/s
  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_speed_data()) =
      SpeedProfileGenerator::GenerateFixedDistanceCreepProfile(
          move_forward_distance, kSidePassCreepSpeed);

  for (const auto& sd : rfl_info.mutable_speed_data()->speed_vector()) {
    ADEBUG << sd.ShortDebugString();
  }

  // (2) combine path and speed.
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
  DiscretizedTrajectory trajectory;
  if (!rfl_info.CombinePathAndSpeedProfile(
          frame->PlanningStartPoint().relative_time(),
          frame->PlanningStartPoint().path_point().s(), &trajectory)) {
    AERROR << "Fail to aggregate planning trajectory.";
    return Stage::RUNNING;
  }
  rfl_info.SetTrajectory(trajectory);
  rfl_info.SetDrivable(true);

  constexpr double kBuffer = 0.3;
  if (move_forward_distance < kBuffer) {
    next_stage_ = ScenarioConfig::SIDE_PASS_DETECT_SAFETY;
  }
  return Stage::FINISHED;
}

bool SidePassStopOnWaitPoint::IsFarAwayFromObstacles(
    const ReferenceLine& reference_line,
    const IndexedList<std::string, Obstacle>& indexed_obstacle_list,
    const PathPoint& first_path_point, const PathPoint& last_path_point) {
  common::SLPoint first_sl_point;

  if (!reference_line.XYToSL(Vec2d(first_path_point.x(), first_path_point.y()),
                             &first_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  common::SLPoint last_sl_point;
  if (!reference_line.XYToSL(Vec2d(last_path_point.x(), last_path_point.y()),
                             &last_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  // Go through every obstacle, check if there is any in the no_obs_zone,
  // which will be used by the proceed_with_caution movement.
  for (const auto* obstacle : indexed_obstacle_list.Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    // Check the s-direction.
    double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
    double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
    if (obs_end_s < first_sl_point.s() ||
        obs_start_s > last_sl_point.s() + kExtraMarginforStopOnWaitPointStage) {
      continue;
    }
    // Check the l-direction.
    double lane_left_width_at_start_s = 0.0;
    double lane_left_width_at_end_s = 0.0;
    double lane_right_width_at_start_s = 0.0;
    double lane_right_width_at_end_s = 0.0;
    reference_line.GetLaneWidth(obs_start_s, &lane_left_width_at_start_s,
                                &lane_right_width_at_start_s);
    reference_line.GetLaneWidth(obs_end_s, &lane_left_width_at_end_s,
                                &lane_right_width_at_end_s);
    double lane_left_width = std::min(std::abs(lane_left_width_at_start_s),
                                      std::abs(lane_left_width_at_end_s));
    double lane_right_width = std::min(std::abs(lane_right_width_at_start_s),
                                       std::abs(lane_right_width_at_end_s));
    double obs_start_l = obstacle->PerceptionSLBoundary().start_l();
    double obs_end_l = obstacle->PerceptionSLBoundary().end_l();
    if (obs_start_l < lane_left_width && -obs_end_l < lane_right_width) {
      return false;
    }
  }
  return true;
}

bool SidePassStopOnWaitPoint::GetTheNearestObstacle(
    const ReferenceLine& reference_line,
    const IndexedList<std::string, Obstacle>& indexed_obstacle_list,
    const Obstacle** nearest_obstacle) {
  // Get the first path point. This can be used later to
  // filter out other obstaces that are behind ADC.
  PathPoint first_path_point =
      GetContext()->path_data_.discretized_path().path_points().front();
  common::SLPoint first_sl_point;
  if (!reference_line.XYToSL(Vec2d(first_path_point.x(), first_path_point.y()),
                             &first_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  // Go through every obstacle.
  bool exist_nearest_obs = false;
  *nearest_obstacle = nullptr;
  double s_of_nearest_obs = 0.0;
  for (const auto* obstacle : indexed_obstacle_list.Items()) {
    ADEBUG << "Looking at Obstacle: " << obstacle->Id();

    if (obstacle->IsVirtual()) {
      continue;
    }

    // Check the s-direction. Rule out those obstacles that are behind ADC.
    double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
    double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
    if (obs_end_s <= first_sl_point.s()) {
      ADEBUG << "Obstacle behind ADC. Rule out.";
      continue;
    }

    // Check the l-direction. Rule out those obstacles that are not
    // blocking our drive-way.
    // TODO(all): should make this a GFLAG because this is also used by
    // side_pass_scenario.cc. They should be consistent.
    constexpr double kLBufferThreshold = 0.3;  // unit: m
    const double driving_width =
        reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());
    const double adc_width =
        VehicleConfigHelper::GetConfig().vehicle_param().width();

    // Refresh the s of the nearest obstacle if needed.
    if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer <=
        kLBufferThreshold) {
    // if (obs_start_l < lane_left_width && -obs_end_l < lane_right_width) {
      ADEBUG << "Obstacle is not completely outside the current lane.";
      if (!exist_nearest_obs) {
        ADEBUG << "Updating the nearest obstacle to: obstacle_"
               << obstacle->Id();
        exist_nearest_obs = true;
        *nearest_obstacle = obstacle;
        s_of_nearest_obs = obs_start_s;
      } else {
        if (obs_start_s < s_of_nearest_obs) {
          ADEBUG << "Updating the nearest obstacle to: obstacle_"
                 << obstacle->Id();
          *nearest_obstacle = obstacle;
          s_of_nearest_obs = obs_start_s;
        }
      }
    }
  }

  return true;
}

bool SidePassStopOnWaitPoint::GetMoveForwardLastPathPoint(
    const ReferenceLine& reference_line, const Obstacle* nearest_obstacle,
    PathPoint* const last_path_point, bool* should_not_move_at_all) {
  *should_not_move_at_all = false;
  int count = 0;

  bool exist_nearest_obs = (nearest_obstacle != nullptr);
  double s_max = 0.0;
  if (exist_nearest_obs) {
    ADEBUG << "There exists a nearest obstacle.";
    s_max = nearest_obstacle->PerceptionSLBoundary().start_s();
  }

  for (const auto& path_point :
       GetContext()->path_data_.discretized_path().path_points()) {
    // Get the four corner points ABCD of ADC at every path point,
    // and keep checking until it gets out of the current lane or
    // reaches the nearest obstacle (in the same lane) ahead.
    const auto& vehicle_box =
        common::VehicleConfigHelper::Instance()->GetBoundingBox(path_point);
    std::vector<Vec2d> ABCDpoints = vehicle_box.GetAllCorners();
    bool is_out_of_curr_lane = false;
    for (size_t i = 0; i < ABCDpoints.size(); i++) {
      // For each corner point, project it onto reference_line
      common::SLPoint curr_point_sl;
      if (!reference_line.XYToSL(ABCDpoints[i], &curr_point_sl)) {
        AERROR << "Failed to get the projection from point onto "
                  "reference_line";
        return false;
      }
      // Get the lane width at the current s indicated by path_point
      double curr_point_left_width = 0.0;
      double curr_point_right_width = 0.0;
      reference_line.GetLaneWidth(curr_point_sl.s(), &curr_point_left_width,
                                  &curr_point_right_width);
      // Check if this corner point is within the lane:
      if (curr_point_sl.l() > std::abs(curr_point_left_width) ||
          curr_point_sl.l() < -std::abs(curr_point_right_width)) {
        is_out_of_curr_lane = true;
        break;
      }
      // Check if this corner point is before the nearest obstacle:
      if (exist_nearest_obs && curr_point_sl.s() > s_max) {
        is_out_of_curr_lane = true;
        break;
      }
    }

    if (is_out_of_curr_lane) {
      if (count == 0) {
        // The current ADC, without moving at all, is already at least
        // partially out of the current lane.
        *should_not_move_at_all = true;
        return true;
      }
      break;
    } else {
      *last_path_point = path_point;
    }

    CHECK_GE(path_point.s(), 0.0);
    ++count;
  }
  return true;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>Planning: removed unused code.<commit_after>/******************************************************************************
 * Copyright 2018 The Apollo 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.
 *****************************************************************************/

/**
 * @file
 **/

#include "modules/planning/scenarios/side_pass/side_pass_stop_on_wait_point.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::PathPoint;
using apollo::common::math::Vec2d;
using apollo::common::VehicleConfigHelper;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

Stage::StageStatus SidePassStopOnWaitPoint::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  ADEBUG << "Processing SidePassStopOnWaitPoint";
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  const PathDecision& path_decision = reference_line_info.path_decision();

  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty.";
    return Stage::ERROR;
  }

  if (!GetContext()->path_data_.UpdateFrenetFramePath(&reference_line)) {
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line.GetFrenetPoint(frame->PlanningStartPoint().path_point());

  if (!GetContext()->path_data_.LeftTrimWithRefS(adc_frenet_frame_point_)) {
    return Stage::ERROR;
  }
  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty after trim.";
    return Stage::ERROR;
  }

  for (const auto& p :
       GetContext()->path_data_.discretized_path().path_points()) {
    ADEBUG << p.ShortDebugString();
  }

  // Get the nearest obstacle
  const Obstacle* nearest_obstacle = nullptr;
  if (!GetTheNearestObstacle(reference_line, path_decision.obstacles(),
                             &nearest_obstacle)) {
    AERROR << "Failed while running the function to get nearest obstacle.";
    return Stage::ERROR;
  }

  // If the nearest obstacle, provided it exists, is moving,
  // then quit the side_pass stage.
  if (nearest_obstacle) {
    if (nearest_obstacle->speed() >
        GetContext()->scenario_config_.block_obstacle_min_speed()) {
      ADEBUG << "The nearest obstacle to side-pass is moving.";
      next_stage_ = ScenarioConfig::NO_STAGE;
      return Stage::FINISHED;
    }
  }

  ADEBUG << "Got the nearest obstacle if there is one.";
  // Get the "wait point".
  PathPoint first_path_point =
      GetContext()->path_data_.discretized_path().path_points().front();
  PathPoint last_path_point;
  bool should_not_move_at_all = false;
  if (!GetMoveForwardLastPathPoint(reference_line, nearest_obstacle,
                                   &last_path_point, &should_not_move_at_all)) {
    ADEBUG << "Fail to get move forward last path point.";
    return Stage::ERROR;
  }
  if (should_not_move_at_all) {
    ADEBUG << "The ADC is already at a stop point.";
    next_stage_ = ScenarioConfig::SIDE_PASS_DETECT_SAFETY;
    return Stage::FINISHED;  // return FINISHED if it's already at "wait point".
  }

  ADEBUG << "first_path_point: " << first_path_point.ShortDebugString();
  ADEBUG << "last_path_point : " << last_path_point.ShortDebugString();
  double move_forward_distance = last_path_point.s() - first_path_point.s();
  ADEBUG << "move_forward_distance: " << move_forward_distance;

  // Wait until everything is clear.
  if (!IsFarAwayFromObstacles(reference_line, path_decision.obstacles(),
                              first_path_point, last_path_point)) {
    // wait here, do nothing this cycle.
    auto& rfl_info = frame->mutable_reference_line_info()->front();
    *(rfl_info.mutable_path_data()) = GetContext()->path_data_;
    *(rfl_info.mutable_speed_data()) =
        SpeedProfileGenerator::GenerateFallbackSpeedProfile();

    rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
    DiscretizedTrajectory trajectory;
    if (!rfl_info.CombinePathAndSpeedProfile(
            frame->PlanningStartPoint().relative_time(),
            frame->PlanningStartPoint().path_point().s(), &trajectory)) {
      AERROR << "Fail to aggregate planning trajectory.";
      return Stage::RUNNING;
    }
    rfl_info.SetTrajectory(trajectory);
    rfl_info.SetDrivable(true);

    ADEBUG << "waiting until obstacles are far away.";
    return Stage::RUNNING;
  }

  // (1) call proceed with cautious
  constexpr double kSidePassCreepSpeed = 2.33;  // m/s
  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_speed_data()) =
      SpeedProfileGenerator::GenerateFixedDistanceCreepProfile(
          move_forward_distance, kSidePassCreepSpeed);

  for (const auto& sd : rfl_info.mutable_speed_data()->speed_vector()) {
    ADEBUG << sd.ShortDebugString();
  }

  // (2) combine path and speed.
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
  DiscretizedTrajectory trajectory;
  if (!rfl_info.CombinePathAndSpeedProfile(
          frame->PlanningStartPoint().relative_time(),
          frame->PlanningStartPoint().path_point().s(), &trajectory)) {
    AERROR << "Fail to aggregate planning trajectory.";
    return Stage::RUNNING;
  }
  rfl_info.SetTrajectory(trajectory);
  rfl_info.SetDrivable(true);

  constexpr double kBuffer = 0.3;
  if (move_forward_distance < kBuffer) {
    next_stage_ = ScenarioConfig::SIDE_PASS_DETECT_SAFETY;
  }
  return Stage::FINISHED;
}

bool SidePassStopOnWaitPoint::IsFarAwayFromObstacles(
    const ReferenceLine& reference_line,
    const IndexedList<std::string, Obstacle>& indexed_obstacle_list,
    const PathPoint& first_path_point, const PathPoint& last_path_point) {
  common::SLPoint first_sl_point;

  if (!reference_line.XYToSL(Vec2d(first_path_point.x(), first_path_point.y()),
                             &first_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  common::SLPoint last_sl_point;
  if (!reference_line.XYToSL(Vec2d(last_path_point.x(), last_path_point.y()),
                             &last_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  // Go through every obstacle, check if there is any in the no_obs_zone,
  // which will be used by the proceed_with_caution movement.
  for (const auto* obstacle : indexed_obstacle_list.Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    // Check the s-direction.
    double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
    double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
    if (obs_end_s < first_sl_point.s() ||
        obs_start_s > last_sl_point.s() + kExtraMarginforStopOnWaitPointStage) {
      continue;
    }
    // Check the l-direction.
    double lane_left_width_at_start_s = 0.0;
    double lane_left_width_at_end_s = 0.0;
    double lane_right_width_at_start_s = 0.0;
    double lane_right_width_at_end_s = 0.0;
    reference_line.GetLaneWidth(obs_start_s, &lane_left_width_at_start_s,
                                &lane_right_width_at_start_s);
    reference_line.GetLaneWidth(obs_end_s, &lane_left_width_at_end_s,
                                &lane_right_width_at_end_s);
    double lane_left_width = std::min(std::abs(lane_left_width_at_start_s),
                                      std::abs(lane_left_width_at_end_s));
    double lane_right_width = std::min(std::abs(lane_right_width_at_start_s),
                                       std::abs(lane_right_width_at_end_s));
    double obs_start_l = obstacle->PerceptionSLBoundary().start_l();
    double obs_end_l = obstacle->PerceptionSLBoundary().end_l();
    if (obs_start_l < lane_left_width && -obs_end_l < lane_right_width) {
      return false;
    }
  }
  return true;
}

bool SidePassStopOnWaitPoint::GetTheNearestObstacle(
    const ReferenceLine& reference_line,
    const IndexedList<std::string, Obstacle>& indexed_obstacle_list,
    const Obstacle** nearest_obstacle) {
  // Get the first path point. This can be used later to
  // filter out other obstaces that are behind ADC.
  PathPoint first_path_point =
      GetContext()->path_data_.discretized_path().path_points().front();
  common::SLPoint first_sl_point;
  if (!reference_line.XYToSL(Vec2d(first_path_point.x(), first_path_point.y()),
                             &first_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  // Go through every obstacle.
  bool exist_nearest_obs = false;
  *nearest_obstacle = nullptr;
  double s_of_nearest_obs = 0.0;
  for (const auto* obstacle : indexed_obstacle_list.Items()) {
    ADEBUG << "Looking at Obstacle: " << obstacle->Id();

    if (obstacle->IsVirtual()) {
      continue;
    }

    // Check the s-direction. Rule out those obstacles that are behind ADC.
    double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
    double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
    if (obs_end_s <= first_sl_point.s()) {
      ADEBUG << "Obstacle behind ADC. Rule out.";
      continue;
    }

    // Check the l-direction. Rule out those obstacles that are not
    // blocking our drive-way.
    // TODO(all): should make this a GFLAG because this is also used by
    // side_pass_scenario.cc. They should be consistent.
    constexpr double kLBufferThreshold = 0.3;  // unit: m
    const double driving_width =
        reference_line.GetDrivingWidth(obstacle->PerceptionSLBoundary());
    const double adc_width =
        VehicleConfigHelper::GetConfig().vehicle_param().width();

    // Refresh the s of the nearest obstacle if needed.
    if (driving_width - adc_width - FLAGS_static_decision_nudge_l_buffer <=
        kLBufferThreshold) {
      ADEBUG << "Obstacle is not completely outside the current lane.";
      if (!exist_nearest_obs) {
        ADEBUG << "Updating the nearest obstacle to: obstacle_"
               << obstacle->Id();
        exist_nearest_obs = true;
        *nearest_obstacle = obstacle;
        s_of_nearest_obs = obs_start_s;
      } else {
        if (obs_start_s < s_of_nearest_obs) {
          ADEBUG << "Updating the nearest obstacle to: obstacle_"
                 << obstacle->Id();
          *nearest_obstacle = obstacle;
          s_of_nearest_obs = obs_start_s;
        }
      }
    }
  }

  return true;
}

bool SidePassStopOnWaitPoint::GetMoveForwardLastPathPoint(
    const ReferenceLine& reference_line, const Obstacle* nearest_obstacle,
    PathPoint* const last_path_point, bool* should_not_move_at_all) {
  *should_not_move_at_all = false;
  int count = 0;

  bool exist_nearest_obs = (nearest_obstacle != nullptr);
  double s_max = 0.0;
  if (exist_nearest_obs) {
    ADEBUG << "There exists a nearest obstacle.";
    s_max = nearest_obstacle->PerceptionSLBoundary().start_s();
  }

  for (const auto& path_point :
       GetContext()->path_data_.discretized_path().path_points()) {
    // Get the four corner points ABCD of ADC at every path point,
    // and keep checking until it gets out of the current lane or
    // reaches the nearest obstacle (in the same lane) ahead.
    const auto& vehicle_box =
        common::VehicleConfigHelper::Instance()->GetBoundingBox(path_point);
    std::vector<Vec2d> ABCDpoints = vehicle_box.GetAllCorners();
    bool is_out_of_curr_lane = false;
    for (size_t i = 0; i < ABCDpoints.size(); i++) {
      // For each corner point, project it onto reference_line
      common::SLPoint curr_point_sl;
      if (!reference_line.XYToSL(ABCDpoints[i], &curr_point_sl)) {
        AERROR << "Failed to get the projection from point onto "
                  "reference_line";
        return false;
      }
      // Get the lane width at the current s indicated by path_point
      double curr_point_left_width = 0.0;
      double curr_point_right_width = 0.0;
      reference_line.GetLaneWidth(curr_point_sl.s(), &curr_point_left_width,
                                  &curr_point_right_width);
      // Check if this corner point is within the lane:
      if (curr_point_sl.l() > std::abs(curr_point_left_width) ||
          curr_point_sl.l() < -std::abs(curr_point_right_width)) {
        is_out_of_curr_lane = true;
        break;
      }
      // Check if this corner point is before the nearest obstacle:
      if (exist_nearest_obs && curr_point_sl.s() > s_max) {
        is_out_of_curr_lane = true;
        break;
      }
    }

    if (is_out_of_curr_lane) {
      if (count == 0) {
        // The current ADC, without moving at all, is already at least
        // partially out of the current lane.
        *should_not_move_at_all = true;
        return true;
      }
      break;
    } else {
      *last_path_point = path_point;
    }

    CHECK_GE(path_point.s(), 0.0);
    ++count;
  }
  return true;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>#include <string>
#include <ostream>
#include <fstream>
#include <sstream>
#include <SDL.h>
#include "../include/logger.h"

Util::Logger::Logger(const std::string &file)
	: fileOut(file.c_str()), output(fileOut)
{
}
Util::Logger::Logger(std::ostream &os) : output(os) {
}
void Util::Logger::log(const std::string &msg){
	timeStamp();
	output << msg;
}
std::string Util::Logger::timeStamp(){
	float sec = SDL_GetTicks() / 1000.f;
	int hrs = sec / 3600;
	int min = sec / 60 - hrs * 3600;
	sec = static_cast<int>(sec - hrs * 3600 - min * 60);
	std::stringstream ss;
	ss << hrs << ":" << min << ":" << sec << "\n";
	return ss.str();
}
Util::Logger& Util::Logger::operator<<(std::ostream&(*f)(std::ostream&)){
	output << f;
	return *this;
}<commit_msg>Removing the endline from the timestamp<commit_after>#include <string>
#include <ostream>
#include <fstream>
#include <sstream>
#include <SDL.h>
#include "../include/logger.h"

Util::Logger::Logger(const std::string &file)
	: fileOut(file.c_str()), output(fileOut)
{
}
Util::Logger::Logger(std::ostream &os) : output(os) {
}
void Util::Logger::log(const std::string &msg){
	timeStamp();
	output << msg;
}
std::string Util::Logger::timeStamp(){
	float sec = SDL_GetTicks() / 1000.f;
	int hrs = sec / 3600;
	int min = sec / 60 - hrs * 3600;
	sec = static_cast<int>(sec - hrs * 3600 - min * 60);
	std::stringstream ss;
	ss << hrs << ":" << min << ":" << sec;
	return ss.str();
}
Util::Logger& Util::Logger::operator<<(std::ostream&(*f)(std::ostream&)){
	output << f;
	return *this;
}<|endoftext|>