<commit_before>#include "cpr/cookies.h"

namespace cpr {
std::string Cookies::GetEncoded(const CurlHolder& holder) const {
    std::stringstream stream;
    for (const std::pair<std::string, std::string>& item : map_) {
        // Depending on if encoding is set to "true", we will URL-encode cookies
        stream << (encode ? holder.urlEncode(item.first) : item.first) << "=";

        // special case version 1 cookies, which can be distinguished by
        // beginning and trailing quotes
        if (!item.second.empty() && item.second.front() == '"' && item.second.back() == '"') {
            stream << item.second;
        } else {
            // Depending on if encoding is set to "true", we will URL-encode cookies
            stream << (encode ? holder.urlEncode(item.second) : item.second);
        }
        stream << "; ";
    }
    return stream.str();
}

std::string& Cookies::operator[](const std::string& key) {
    return map_[key];
}

Cookies::iterator Cookies::begin() {
    return map_.begin();
}

Cookies::iterator Cookies::end() {
    return map_.end();
}

Cookies::const_iterator Cookies::begin() const {
    return map_.begin();
}

Cookies::const_iterator Cookies::end() const {
    return map_.end();
}

Cookies::const_iterator Cookies::cbegin() const {
    return map_.cbegin();
}

Cookies::const_iterator Cookies::cend() const {
    return map_.cend();
}

} // namespace cpr
<commit_msg>Fixed implicit conversion<commit_after>#include "cpr/cookies.h"

namespace cpr {
std::string Cookies::GetEncoded(const CurlHolder& holder) const {
    std::stringstream stream;
    for (const auto& item : map_) {
        // Depending on if encoding is set to "true", we will URL-encode cookies
        stream << (encode ? holder.urlEncode(item.first) : item.first) << "=";

        // special case version 1 cookies, which can be distinguished by
        // beginning and trailing quotes
        if (!item.second.empty() && item.second.front() == '"' && item.second.back() == '"') {
            stream << item.second;
        } else {
            // Depending on if encoding is set to "true", we will URL-encode cookies
            stream << (encode ? holder.urlEncode(item.second) : item.second);
        }
        stream << "; ";
    }
    return stream.str();
}

std::string& Cookies::operator[](const std::string& key) {
    return map_[key];
}

Cookies::iterator Cookies::begin() {
    return map_.begin();
}

Cookies::iterator Cookies::end() {
    return map_.end();
}

Cookies::const_iterator Cookies::begin() const {
    return map_.begin();
}

Cookies::const_iterator Cookies::end() const {
    return map_.end();
}

Cookies::const_iterator Cookies::cbegin() const {
    return map_.cbegin();
}

Cookies::const_iterator Cookies::cend() const {
    return map_.cend();
}

} // namespace cpr
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2003 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <string>
#include <sstream>
#include <iostream>

#include "cpu/base_cpu.hh"
#include "base/cprintf.hh"
#include "cpu/exec_context.hh"
#include "base/misc.hh"
#include "sim/param.hh"
#include "sim/sim_events.hh"

using namespace std;

vector<BaseCPU *> BaseCPU::cpuList;

// This variable reflects the max number of threads in any CPU.  Be
// careful to only use it once all the CPUs that you care about have
// been initialized
int maxThreadsPerCPU = 1;

#ifdef FULL_SYSTEM
BaseCPU::BaseCPU(const string &_name, int _number_of_threads,
                 Counter max_insts_any_thread,
                 Counter max_insts_all_threads,
                 Counter max_loads_any_thread,
                 Counter max_loads_all_threads,
                 System *_system, Tick freq)
    : SimObject(_name), frequency(freq),
      number_of_threads(_number_of_threads), system(_system)
#else
BaseCPU::BaseCPU(const string &_name, int _number_of_threads,
                 Counter max_insts_any_thread,
                 Counter max_insts_all_threads,
                 Counter max_loads_any_thread,
                 Counter max_loads_all_threads)
    : SimObject(_name), number_of_threads(_number_of_threads)
#endif
{
    // add self to global list of CPUs
    cpuList.push_back(this);

    if (number_of_threads > maxThreadsPerCPU)
        maxThreadsPerCPU = number_of_threads;

    // allocate per-thread instruction-based event queues
    comInstEventQueue = new (EventQueue *)[number_of_threads];
    for (int i = 0; i < number_of_threads; ++i)
        comInstEventQueue[i] = new EventQueue("instruction-based event queue");

    //
    // set up instruction-count-based termination events, if any
    //
    if (max_insts_any_thread != 0)
        for (int i = 0; i < number_of_threads; ++i)
            new SimExitEvent(comInstEventQueue[i], max_insts_any_thread,
                "a thread reached the max instruction count");

    if (max_insts_all_threads != 0) {
        // allocate & initialize shared downcounter: each event will
        // decrement this when triggered; simulation will terminate
        // when counter reaches 0
        int *counter = new int;
        *counter = number_of_threads;
        for (int i = 0; i < number_of_threads; ++i)
            new CountedExitEvent(comInstEventQueue[i],
                "all threads reached the max instruction count",
                max_insts_all_threads, *counter);
    }

    // allocate per-thread load-based event queues
    comLoadEventQueue = new (EventQueue *)[number_of_threads];
    for (int i = 0; i < number_of_threads; ++i)
        comLoadEventQueue[i] = new EventQueue("load-based event queue");

    //
    // set up instruction-count-based termination events, if any
    //
    if (max_loads_any_thread != 0)
        for (int i = 0; i < number_of_threads; ++i)
            new SimExitEvent(comLoadEventQueue[i], max_loads_any_thread,
                "a thread reached the max load count");

    if (max_loads_all_threads != 0) {
        // allocate & initialize shared downcounter: each event will
        // decrement this when triggered; simulation will terminate
        // when counter reaches 0
        int *counter = new int;
        *counter = number_of_threads;
        for (int i = 0; i < number_of_threads; ++i)
            new CountedExitEvent(comLoadEventQueue[i],
                "all threads reached the max load count",
                max_loads_all_threads, *counter);
    }

#ifdef FULL_SYSTEM
    memset(interrupts, 0, sizeof(interrupts));
    intstatus = 0;
#endif
}

void
BaseCPU::regStats()
{
    int size = execContexts.size();
    if (size > 1) {
        for (int i = 0; i < size; ++i) {
            stringstream namestr;
            ccprintf(namestr, "%s.ctx%d", name(), i);
            execContexts[i]->regStats(namestr.str());
        }
    } else if (size == 1)
        execContexts[0]->regStats(name());
}

void
BaseCPU::registerExecContexts()
{
    for (int i = 0; i < execContexts.size(); ++i) {
        ExecContext *xc = execContexts[i];
        int cpu_id;

#ifdef FULL_SYSTEM
        cpu_id = system->registerExecContext(xc);
#else
        cpu_id = xc->process->registerExecContext(xc);
#endif

        xc->cpu_id = cpu_id;
    }
}

void
BaseCPU::switchOut()
{
    // default: do nothing
}

void
BaseCPU::takeOverFrom(BaseCPU *oldCPU)
{
    assert(execContexts.size() == oldCPU->execContexts.size());

    for (int i = 0; i < execContexts.size(); ++i) {
        ExecContext *newXC = execContexts[i];
        ExecContext *oldXC = oldCPU->execContexts[i];

        newXC->takeOverFrom(oldXC);
        assert(newXC->cpu_id == oldXC->cpu_id);
#ifdef FULL_SYSTEM
        system->replaceExecContext(newXC->cpu_id, newXC);
#else
        assert(newXC->process == oldXC->process);
        newXC->process->replaceExecContext(newXC->cpu_id, newXC);
#endif
    }
}

#ifdef FULL_SYSTEM
void
BaseCPU::post_interrupt(int int_num, int index)
{
    DPRINTF(Interrupt, "Interrupt %d:%d posted\n", int_num, index);

    if (int_num < 0 || int_num >= NumInterruptLevels)
        panic("int_num out of bounds\n");

    if (index < 0 || index >= sizeof(uint8_t) * 8)
        panic("int_num out of bounds\n");

    AlphaISA::check_interrupts = 1;
    interrupts[int_num] |= 1 << index;
    intstatus |= (ULL(1) << int_num);
}

void
BaseCPU::clear_interrupt(int int_num, int index)
{
    DPRINTF(Interrupt, "Interrupt %d:%d cleared\n", int_num, index);

    if (int_num < 0 || int_num >= NumInterruptLevels)
        panic("int_num out of bounds\n");

    if (index < 0 || index >= sizeof(uint8_t) * 8)
        panic("int_num out of bounds\n");

    interrupts[int_num] &= ~(1 << index);
    if (interrupts[int_num] == 0)
        intstatus &= ~(ULL(1) << int_num);
}

void
BaseCPU::clear_interrupts()
{
    DPRINTF(Interrupt, "Interrupts all cleared\n");

    memset(interrupts, 0, sizeof(interrupts));
    intstatus = 0;
}

#endif // FULL_SYSTEM

//
// This declaration is not needed now that SamplingCPU provides a
// BaseCPUBuilder object.
//
#if 0
DEFINE_SIM_OBJECT_CLASS_NAME("BaseCPU", BaseCPU)
#endif
<commit_msg>fix switchover WRT interrupts<commit_after>/*
 * Copyright (c) 2003 The Regents of The University of Michigan
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met: redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer;
 * redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution;
 * neither the name of the copyright holders nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <string>
#include <sstream>
#include <iostream>

#include "cpu/base_cpu.hh"
#include "base/cprintf.hh"
#include "cpu/exec_context.hh"
#include "base/misc.hh"
#include "sim/param.hh"
#include "sim/sim_events.hh"

using namespace std;

vector<BaseCPU *> BaseCPU::cpuList;

// This variable reflects the max number of threads in any CPU.  Be
// careful to only use it once all the CPUs that you care about have
// been initialized
int maxThreadsPerCPU = 1;

#ifdef FULL_SYSTEM
BaseCPU::BaseCPU(const string &_name, int _number_of_threads,
                 Counter max_insts_any_thread,
                 Counter max_insts_all_threads,
                 Counter max_loads_any_thread,
                 Counter max_loads_all_threads,
                 System *_system, Tick freq)
    : SimObject(_name), frequency(freq),
      number_of_threads(_number_of_threads), system(_system)
#else
BaseCPU::BaseCPU(const string &_name, int _number_of_threads,
                 Counter max_insts_any_thread,
                 Counter max_insts_all_threads,
                 Counter max_loads_any_thread,
                 Counter max_loads_all_threads)
    : SimObject(_name), number_of_threads(_number_of_threads)
#endif
{
    // add self to global list of CPUs
    cpuList.push_back(this);

    if (number_of_threads > maxThreadsPerCPU)
        maxThreadsPerCPU = number_of_threads;

    // allocate per-thread instruction-based event queues
    comInstEventQueue = new (EventQueue *)[number_of_threads];
    for (int i = 0; i < number_of_threads; ++i)
        comInstEventQueue[i] = new EventQueue("instruction-based event queue");

    //
    // set up instruction-count-based termination events, if any
    //
    if (max_insts_any_thread != 0)
        for (int i = 0; i < number_of_threads; ++i)
            new SimExitEvent(comInstEventQueue[i], max_insts_any_thread,
                "a thread reached the max instruction count");

    if (max_insts_all_threads != 0) {
        // allocate & initialize shared downcounter: each event will
        // decrement this when triggered; simulation will terminate
        // when counter reaches 0
        int *counter = new int;
        *counter = number_of_threads;
        for (int i = 0; i < number_of_threads; ++i)
            new CountedExitEvent(comInstEventQueue[i],
                "all threads reached the max instruction count",
                max_insts_all_threads, *counter);
    }

    // allocate per-thread load-based event queues
    comLoadEventQueue = new (EventQueue *)[number_of_threads];
    for (int i = 0; i < number_of_threads; ++i)
        comLoadEventQueue[i] = new EventQueue("load-based event queue");

    //
    // set up instruction-count-based termination events, if any
    //
    if (max_loads_any_thread != 0)
        for (int i = 0; i < number_of_threads; ++i)
            new SimExitEvent(comLoadEventQueue[i], max_loads_any_thread,
                "a thread reached the max load count");

    if (max_loads_all_threads != 0) {
        // allocate & initialize shared downcounter: each event will
        // decrement this when triggered; simulation will terminate
        // when counter reaches 0
        int *counter = new int;
        *counter = number_of_threads;
        for (int i = 0; i < number_of_threads; ++i)
            new CountedExitEvent(comLoadEventQueue[i],
                "all threads reached the max load count",
                max_loads_all_threads, *counter);
    }

#ifdef FULL_SYSTEM
    memset(interrupts, 0, sizeof(interrupts));
    intstatus = 0;
#endif
}

void
BaseCPU::regStats()
{
    int size = execContexts.size();
    if (size > 1) {
        for (int i = 0; i < size; ++i) {
            stringstream namestr;
            ccprintf(namestr, "%s.ctx%d", name(), i);
            execContexts[i]->regStats(namestr.str());
        }
    } else if (size == 1)
        execContexts[0]->regStats(name());
}

void
BaseCPU::registerExecContexts()
{
    for (int i = 0; i < execContexts.size(); ++i) {
        ExecContext *xc = execContexts[i];
        int cpu_id;

#ifdef FULL_SYSTEM
        cpu_id = system->registerExecContext(xc);
#else
        cpu_id = xc->process->registerExecContext(xc);
#endif

        xc->cpu_id = cpu_id;
    }
}

void
BaseCPU::switchOut()
{
    // default: do nothing
}

void
BaseCPU::takeOverFrom(BaseCPU *oldCPU)
{
    assert(execContexts.size() == oldCPU->execContexts.size());

    for (int i = 0; i < execContexts.size(); ++i) {
        ExecContext *newXC = execContexts[i];
        ExecContext *oldXC = oldCPU->execContexts[i];

        newXC->takeOverFrom(oldXC);
        assert(newXC->cpu_id == oldXC->cpu_id);
#ifdef FULL_SYSTEM
        system->replaceExecContext(newXC->cpu_id, newXC);
#else
        assert(newXC->process == oldXC->process);
        newXC->process->replaceExecContext(newXC->cpu_id, newXC);
#endif
    }

    for (int i = 0; i < NumInterruptLevels; ++i)
        interrupts[i] = oldCPU->interrupts[i];
    intstatus = oldCPU->intstatus;
}

#ifdef FULL_SYSTEM
void
BaseCPU::post_interrupt(int int_num, int index)
{
    DPRINTF(Interrupt, "Interrupt %d:%d posted\n", int_num, index);

    if (int_num < 0 || int_num >= NumInterruptLevels)
        panic("int_num out of bounds\n");

    if (index < 0 || index >= sizeof(uint8_t) * 8)
        panic("int_num out of bounds\n");

    AlphaISA::check_interrupts = 1;
    interrupts[int_num] |= 1 << index;
    intstatus |= (ULL(1) << int_num);
}

void
BaseCPU::clear_interrupt(int int_num, int index)
{
    DPRINTF(Interrupt, "Interrupt %d:%d cleared\n", int_num, index);

    if (int_num < 0 || int_num >= NumInterruptLevels)
        panic("int_num out of bounds\n");

    if (index < 0 || index >= sizeof(uint8_t) * 8)
        panic("int_num out of bounds\n");

    interrupts[int_num] &= ~(1 << index);
    if (interrupts[int_num] == 0)
        intstatus &= ~(ULL(1) << int_num);
}

void
BaseCPU::clear_interrupts()
{
    DPRINTF(Interrupt, "Interrupts all cleared\n");

    memset(interrupts, 0, sizeof(interrupts));
    intstatus = 0;
}

#endif // FULL_SYSTEM

//
// This declaration is not needed now that SamplingCPU provides a
// BaseCPUBuilder object.
//
#if 0
DEFINE_SIM_OBJECT_CLASS_NAME("BaseCPU", BaseCPU)
#endif
<|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.
 *****************************************************************************/

#include "Eigen/Dense"
#include "gflags/gflags.h"
#include "pcl/io/pcd_io.h"
#include "pcl/kdtree/impl/kdtree_flann.hpp"
#include "pcl/kdtree/kdtree.h"
#include "pcl/kdtree/kdtree_flann.h"

#include "modules/common/util/file.h"
#include "modules/perception/base/object.h"
#include "modules/perception/base/object_types.h"
#include "modules/perception/base/point_cloud.h"
#include "modules/perception/common/io/io_util.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/common/point_cloud_processing/common.h"
#include "modules/perception/common/sensor_manager/sensor_manager.h"
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lidar/app/lidar_obstacle_segmentation.h"
#include "modules/perception/lidar/app/lidar_obstacle_tracking.h"
#include "modules/perception/lidar/common/lidar_frame.h"
#include "modules/perception/lidar/common/lidar_frame_pool.h"
#include "modules/perception/lidar/common/lidar_log.h"
#include "modules/perception/lidar/common/pcl_util.h"

DEFINE_string(pcd_path, "./pcd/", "pcd path");
DEFINE_string(pose_path, "", "pose path");
DEFINE_string(output_path, "./output/", "output path");
DEFINE_bool(enable_tracking, false, "option to enable tracking");
DEFINE_double(min_life_time, -1.0, "minimum track time for output");
DEFINE_bool(use_hdmap, false, "option to enable using hdmap");
DEFINE_bool(use_tracking_info, false, "option to use tracking info");
DEFINE_string(sensor_name, "velodyne64", "sensor name");

namespace apollo {
namespace perception {
namespace lidar {

using apollo::common::util::GetFileName;

class OfflineLidarObstaclePerception {
 public:
  OfflineLidarObstaclePerception() = default;

  ~OfflineLidarObstaclePerception() = default;

  bool setup() {
    FLAGS_config_manager_path = "./conf";
    if (!lib::ConfigManager::Instance()->Init()) {
      AERROR << "Failed to init ConfigManage.";
      return false;
    }
    lidar_segmentation_.reset(new LidarObstacleSegmentation);
    if (lidar_segmentation_ == nullptr) {
      AERROR << "Failed to get LidarObstacleSegmentation instance.";
      return false;
    }
    segment_init_options_.enable_hdmap_input = FLAGS_use_hdmap;
    segment_init_options_.sensor_name = FLAGS_sensor_name;
    if (!lidar_segmentation_->Init(segment_init_options_)) {
      AINFO << "Failed to init LidarObstacleSegmentation.";
      return false;
    }
    lidar_tracking_.reset(new LidarObstacleTracking);
    if (lidar_tracking_ == nullptr) {
      AERROR << "Failed to get LidarObstacleTracking instance.";
      return false;
    }
    tracking_init_options_.sensor_name = FLAGS_sensor_name;
    if (!lidar_tracking_->Init(tracking_init_options_)) {
      AINFO << "Failed to init LidarObstacleSegmentation.";
      return false;
    }

    bool ret = common::SensorManager::Instance()->GetSensorInfo(
        FLAGS_sensor_name, &sensor_info_);
    if (!ret) {
      AERROR << "Failed to get sensor info, sensor name: "
        << FLAGS_sensor_name;
      return false;
    }
    ADEBUG << "Sensor_name: " << sensor_info_.name;

    return true;
  }

  void run() {
    double timestamp = 0.f;
    std::string pcd_folder = FLAGS_pcd_path;
    std::string pose_folder = FLAGS_pose_path;
    std::string output_path = FLAGS_output_path;
    std::vector<std::string> pcd_file_names;
    common::GetFileList(pcd_folder, ".pcd", &pcd_file_names);
    std::sort(pcd_file_names.begin(), pcd_file_names.end(),
              [](const std::string& lhs, const std::string& rhs) {
                if (lhs.length() != rhs.length()) {
                  return lhs.length() < rhs.length();
                }
                return lhs <= rhs;
              });
    for (size_t i = 0; i < pcd_file_names.size(); i++) {
      AINFO << "***************** Frame " << i << " ******************";
      AINFO << pcd_file_names[i];
      const std::string file_name = GetFileName(pcd_file_names[i]);
      frame_ = LidarFramePool::Instance().Get();
      frame_->sensor_info = sensor_info_;
      frame_->reserve = file_name;
      if (frame_->cloud == nullptr) {
        frame_->cloud = base::PointFCloudPool::Instance().Get();
      }
      LoadPCLPCD(pcd_folder + "/" + file_name + ".pcd", frame_->cloud.get());
      AINFO << "Read point cloud from " << pcd_file_names[i]
               << " with cloud size: " << frame_->cloud->size();
      if (pose_folder != "") {
        std::string pose_file_name = pose_folder + "/" + file_name + ".pose";
        AINFO << "Pose file: " << pose_file_name;
        if (!apollo::common::util::PathExists(pose_file_name)) {
          pose_file_name = pose_folder + "/" + file_name + ".pcd.pose";
        }
        int idt = 0;
        if (common::ReadPoseFile(pose_file_name, &frame_->lidar2world_pose,
                                 &idt, &timestamp)) {
          AINFO << "[timestamp]: " << std::setprecision(16) << timestamp;
          frame_->timestamp = timestamp;
        } else {
          AINFO << "Failed to load pose, disable tracking pipeline.";
          FLAGS_enable_tracking = false;
        }
      }
      // TODO(shitingmin) undo timestamp.
      LidarProcessResult segment_result =
          lidar_segmentation_->Process(segment_options_, frame_.get());
      if (segment_result.error_code != LidarErrorCode::Succeed) {
        AINFO << segment_result.log;
        return;
      }
      if (FLAGS_enable_tracking) {
        AINFO << "Enable tracking.";
        LidarProcessResult tracking_result =
            lidar_tracking_->Process(tracking_options_, frame_.get());
        if (tracking_result.error_code != LidarErrorCode::Succeed) {
          AINFO << tracking_result.log;
          return;
        }
        if (FLAGS_use_tracking_info) {
          auto& objects = frame_->segmented_objects;
          auto& result_objects = frame_->tracked_objects;
          std::sort(objects.begin(), objects.end(),
                    [](const base::ObjectPtr& lhs, const base::ObjectPtr& rhs) {
                      return lhs->id < rhs->id;
                    });
          std::sort(result_objects.begin(), result_objects.end(),
                    [](const base::ObjectPtr& lhs, const base::ObjectPtr& rhs) {
                      return lhs->id < rhs->id;
                    });
          for (std::size_t j = 0; j < objects.size(); ++j) {
            CHECK(objects[j]->id == result_objects[j]->id);
            objects[j]->track_id = result_objects[j]->track_id;
            objects[j]->tracking_time = result_objects[j]->tracking_time;
            objects[j]->center =
                frame_->lidar2world_pose.inverse() * result_objects[j]->center;
            Eigen::Vector3d direction =
                result_objects[j]->direction.cast<double>();
            direction =
                frame_->lidar2world_pose.linear().transpose() * direction;
            objects[j]->direction = direction.cast<float>();
            objects[j]->size = result_objects[j]->size;
            auto velocity = frame_->lidar2world_pose.linear().transpose() *
                            result_objects[j]->velocity.cast<double>();
            objects[j]->velocity = velocity.cast<float>();

            objects[j]->type = result_objects[j]->type;
            objects[j]->type_probs = result_objects[j]->type_probs;
            objects[j]->polygon = result_objects[j]->polygon;
            common::TransformPointCloud(result_objects[j]->polygon,
                                        frame_->lidar2world_pose.inverse(),
                                        &objects[j]->polygon);
          }
        }
      }

      std::vector<base::ObjectPtr>& result_objects = frame_->segmented_objects;
      std::vector<base::ObjectPtr> filtered_objects;
      for (auto& object : result_objects) {
        if (object->tracking_time >= FLAGS_min_life_time) {
          filtered_objects.push_back(object);
        }
      }
      WriteObjectsForNewBenchmark(i, filtered_objects,
                                  output_path + "/" + file_name + ".pcd");
    }
  }

  bool WriteObjectsForNewBenchmark(size_t frame_id,
                                   const std::vector<base::ObjectPtr>& objects,
                                   const std::string& path) {
    std::ofstream fout(path);
    if (!fout.is_open()) {
      AERROR << "Fail to open " << path << std::endl;
      return false;
    }
    fout << frame_id << " " << objects.size() << std::endl;
    for (auto& object : objects) {
      fout << "velodyne_64"
           << " " << object->id << " " << object->track_id << " "
           << object->lidar_supplement.is_background << " "
           << std::setprecision(8) << object->confidence << " ";

      std::string type = "unknow";
      if (object->type == base::ObjectType::UNKNOWN ||
          object->type == base::ObjectType::UNKNOWN_MOVABLE ||
          object->type == base::ObjectType::UNKNOWN_UNMOVABLE) {
        type = "unknow";
      } else if (object->type == base::ObjectType::PEDESTRIAN) {
        type = "pedestrian";
      } else if (object->type == base::ObjectType::VEHICLE) {
        type = "smallMot";
      } else if (object->type == base::ObjectType::BICYCLE) {
        type = "nonMot";
      }

      double yaw = atan2(object->direction[1], object->direction[0]);
      auto& object_cloud = object->lidar_supplement.cloud;

      fout << type << " " << object->center(0) << " " << object->center(1)
           << " " << object->center(2) << " " << object->size(0) << " "
           << object->size(1) << " " << object->size(2) << " "
           << yaw /*yaw*/ << " " << 0 /*roll*/ << " " << 0 /*pitch*/ << " "
           << 0 /*truncated*/ << " " << 0 /*occluded*/ << " "
           << object->velocity(0) << " " << object->velocity(1) << " "
           << object->velocity(2) << " ";
      fout << object_cloud.size() << " ";
      for (size_t i = 0; i < object_cloud.size(); ++i) {
        auto& pt = object_cloud.at(i);
        fout << std::setprecision(8) << pt.x << " " << pt.y << " " << pt.z
             << " " << pt.intensity << " ";
      }
      // fout << 0/*#indices*/ << " ";
      // we save polygon here for debug
      int polygon_size = -static_cast<int>(object->polygon.size());
      fout << polygon_size /*negative polygon size*/ << " ";
      for (size_t i = 0; i < object->polygon.size(); ++i) {
        const auto& pt = object->polygon[i];
        fout << std::setprecision(8) << pt.x << " " << pt.y << " " << pt.z
             << " ";
      }
      fout << std::endl;
    }
    // we save road boundary and road/junction polygon here for debug
    if (frame_->hdmap_struct != nullptr) {
      // transform to local frame, note since missing valid z value, the local
      // coordinates may not be accurate, only for visualization purpose
      double z = frame_->lidar2world_pose(2, 3) - 1.7;
      Eigen::Affine3d world2lidar_pose = frame_->lidar2world_pose.inverse();
      auto write_transformed_polygon = [&](const base::PolygonDType& poly) {
        Eigen::Vector3d point;
        fout << poly.size() << " ";
        for (size_t i = 0; i < poly.size(); ++i) {
          const auto& pt = poly[i];
          point << pt.x, pt.y, z;
          point = world2lidar_pose * point;
          fout << std::setprecision(8) << point(0) << " " << point(1) << " "
               << point(2) << " ";
        }
        fout << std::endl;
      };
      // 1. write road boundary
      fout << frame_->hdmap_struct->road_boundary.size() << std::endl;
      for (auto& pair : frame_->hdmap_struct->road_boundary) {
        write_transformed_polygon(pair.left_boundary);
        write_transformed_polygon(pair.right_boundary);
      }
      // 2. write road/junction polygon
      fout << frame_->hdmap_struct->road_polygons.size() +
                  frame_->hdmap_struct->junction_polygons.size()
           << std::endl;
      for (auto& poly : frame_->hdmap_struct->road_polygons) {
        write_transformed_polygon(poly);
      }
      for (auto& poly : frame_->hdmap_struct->junction_polygons) {
        write_transformed_polygon(poly);
      }
      // 3. save points label
      fout << frame_->cloud->size() << " ";
      for (size_t i = 0; i < frame_->cloud->size(); ++i) {
        fout << static_cast<int>(frame_->cloud->points_label(i)) << " ";
      }
      fout << std::endl;
    }
    fout.close();
    return true;
  }

 protected:
  std::string output_dir_;
  std::shared_ptr<LidarFrame> frame_;
  LidarObstacleSegmentationInitOptions segment_init_options_;
  LidarObstacleSegmentationOptions segment_options_;
  LidarObstacleTrackingInitOptions tracking_init_options_;
  LidarObstacleTrackingOptions tracking_options_;
  std::unique_ptr<LidarObstacleSegmentation> lidar_segmentation_;
  std::unique_ptr<LidarObstacleTracking> lidar_tracking_;
  base::SensorInfo sensor_info_;
};

}  // namespace lidar
}  // namespace perception
}  // namespace apollo

int main(int argc, char* argv[]) {
  FLAGS_alsologtostderr = 1;
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  apollo::perception::lidar::OfflineLidarObstaclePerception test;
  if (!test.setup()) {
    AINFO << "Failed to setup OfflineLidarObstaclePerception";
    return 0;
  }
  test.run();
  return 0;
}
<commit_msg>perception: remove unnecessary type value re-assignment<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.
 *****************************************************************************/

#include "Eigen/Dense"
#include "gflags/gflags.h"
#include "pcl/io/pcd_io.h"
#include "pcl/kdtree/impl/kdtree_flann.hpp"
#include "pcl/kdtree/kdtree.h"
#include "pcl/kdtree/kdtree_flann.h"

#include "modules/common/util/file.h"
#include "modules/perception/base/object.h"
#include "modules/perception/base/object_types.h"
#include "modules/perception/base/point_cloud.h"
#include "modules/perception/common/io/io_util.h"
#include "modules/perception/common/perception_gflags.h"
#include "modules/perception/common/point_cloud_processing/common.h"
#include "modules/perception/common/sensor_manager/sensor_manager.h"
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lidar/app/lidar_obstacle_segmentation.h"
#include "modules/perception/lidar/app/lidar_obstacle_tracking.h"
#include "modules/perception/lidar/common/lidar_frame.h"
#include "modules/perception/lidar/common/lidar_frame_pool.h"
#include "modules/perception/lidar/common/lidar_log.h"
#include "modules/perception/lidar/common/pcl_util.h"

DEFINE_string(pcd_path, "./pcd/", "pcd path");
DEFINE_string(pose_path, "", "pose path");
DEFINE_string(output_path, "./output/", "output path");
DEFINE_bool(enable_tracking, false, "option to enable tracking");
DEFINE_double(min_life_time, -1.0, "minimum track time for output");
DEFINE_bool(use_hdmap, false, "option to enable using hdmap");
DEFINE_bool(use_tracking_info, false, "option to use tracking info");
DEFINE_string(sensor_name, "velodyne64", "sensor name");

namespace apollo {
namespace perception {
namespace lidar {

using apollo::common::util::GetFileName;

class OfflineLidarObstaclePerception {
 public:
  OfflineLidarObstaclePerception() = default;

  ~OfflineLidarObstaclePerception() = default;

  bool setup() {
    FLAGS_config_manager_path = "./conf";
    if (!lib::ConfigManager::Instance()->Init()) {
      AERROR << "Failed to init ConfigManage.";
      return false;
    }
    lidar_segmentation_.reset(new LidarObstacleSegmentation);
    if (lidar_segmentation_ == nullptr) {
      AERROR << "Failed to get LidarObstacleSegmentation instance.";
      return false;
    }
    segment_init_options_.enable_hdmap_input = FLAGS_use_hdmap;
    segment_init_options_.sensor_name = FLAGS_sensor_name;
    if (!lidar_segmentation_->Init(segment_init_options_)) {
      AINFO << "Failed to init LidarObstacleSegmentation.";
      return false;
    }
    lidar_tracking_.reset(new LidarObstacleTracking);
    if (lidar_tracking_ == nullptr) {
      AERROR << "Failed to get LidarObstacleTracking instance.";
      return false;
    }
    tracking_init_options_.sensor_name = FLAGS_sensor_name;
    if (!lidar_tracking_->Init(tracking_init_options_)) {
      AINFO << "Failed to init LidarObstacleSegmentation.";
      return false;
    }

    bool ret = common::SensorManager::Instance()->GetSensorInfo(
        FLAGS_sensor_name, &sensor_info_);
    if (!ret) {
      AERROR << "Failed to get sensor info, sensor name: "
        << FLAGS_sensor_name;
      return false;
    }
    ADEBUG << "Sensor_name: " << sensor_info_.name;

    return true;
  }

  void run() {
    double timestamp = 0.f;
    std::string pcd_folder = FLAGS_pcd_path;
    std::string pose_folder = FLAGS_pose_path;
    std::string output_path = FLAGS_output_path;
    std::vector<std::string> pcd_file_names;
    common::GetFileList(pcd_folder, ".pcd", &pcd_file_names);
    std::sort(pcd_file_names.begin(), pcd_file_names.end(),
              [](const std::string& lhs, const std::string& rhs) {
                if (lhs.length() != rhs.length()) {
                  return lhs.length() < rhs.length();
                }
                return lhs <= rhs;
              });
    for (size_t i = 0; i < pcd_file_names.size(); i++) {
      AINFO << "***************** Frame " << i << " ******************";
      AINFO << pcd_file_names[i];
      const std::string file_name = GetFileName(pcd_file_names[i]);
      frame_ = LidarFramePool::Instance().Get();
      frame_->sensor_info = sensor_info_;
      frame_->reserve = file_name;
      if (frame_->cloud == nullptr) {
        frame_->cloud = base::PointFCloudPool::Instance().Get();
      }
      LoadPCLPCD(pcd_folder + "/" + file_name + ".pcd", frame_->cloud.get());
      AINFO << "Read point cloud from " << pcd_file_names[i]
               << " with cloud size: " << frame_->cloud->size();
      if (pose_folder != "") {
        std::string pose_file_name = pose_folder + "/" + file_name + ".pose";
        AINFO << "Pose file: " << pose_file_name;
        if (!apollo::common::util::PathExists(pose_file_name)) {
          pose_file_name = pose_folder + "/" + file_name + ".pcd.pose";
        }
        int idt = 0;
        if (common::ReadPoseFile(pose_file_name, &frame_->lidar2world_pose,
                                 &idt, &timestamp)) {
          AINFO << "[timestamp]: " << std::setprecision(16) << timestamp;
          frame_->timestamp = timestamp;
        } else {
          AINFO << "Failed to load pose, disable tracking pipeline.";
          FLAGS_enable_tracking = false;
        }
      }
      // TODO(shitingmin) undo timestamp.
      LidarProcessResult segment_result =
          lidar_segmentation_->Process(segment_options_, frame_.get());
      if (segment_result.error_code != LidarErrorCode::Succeed) {
        AINFO << segment_result.log;
        return;
      }
      if (FLAGS_enable_tracking) {
        AINFO << "Enable tracking.";
        LidarProcessResult tracking_result =
            lidar_tracking_->Process(tracking_options_, frame_.get());
        if (tracking_result.error_code != LidarErrorCode::Succeed) {
          AINFO << tracking_result.log;
          return;
        }
        if (FLAGS_use_tracking_info) {
          auto& objects = frame_->segmented_objects;
          auto& result_objects = frame_->tracked_objects;
          std::sort(objects.begin(), objects.end(),
                    [](const base::ObjectPtr& lhs, const base::ObjectPtr& rhs) {
                      return lhs->id < rhs->id;
                    });
          std::sort(result_objects.begin(), result_objects.end(),
                    [](const base::ObjectPtr& lhs, const base::ObjectPtr& rhs) {
                      return lhs->id < rhs->id;
                    });
          for (std::size_t j = 0; j < objects.size(); ++j) {
            CHECK(objects[j]->id == result_objects[j]->id);
            objects[j]->track_id = result_objects[j]->track_id;
            objects[j]->tracking_time = result_objects[j]->tracking_time;
            objects[j]->center =
                frame_->lidar2world_pose.inverse() * result_objects[j]->center;
            Eigen::Vector3d direction =
                result_objects[j]->direction.cast<double>();
            direction =
                frame_->lidar2world_pose.linear().transpose() * direction;
            objects[j]->direction = direction.cast<float>();
            objects[j]->size = result_objects[j]->size;
            auto velocity = frame_->lidar2world_pose.linear().transpose() *
                            result_objects[j]->velocity.cast<double>();
            objects[j]->velocity = velocity.cast<float>();

            objects[j]->type = result_objects[j]->type;
            objects[j]->type_probs = result_objects[j]->type_probs;
            objects[j]->polygon = result_objects[j]->polygon;
            common::TransformPointCloud(result_objects[j]->polygon,
                                        frame_->lidar2world_pose.inverse(),
                                        &objects[j]->polygon);
          }
        }
      }

      std::vector<base::ObjectPtr>& result_objects = frame_->segmented_objects;
      std::vector<base::ObjectPtr> filtered_objects;
      for (auto& object : result_objects) {
        if (object->tracking_time >= FLAGS_min_life_time) {
          filtered_objects.push_back(object);
        }
      }
      WriteObjectsForNewBenchmark(i, filtered_objects,
                                  output_path + "/" + file_name + ".pcd");
    }
  }

  bool WriteObjectsForNewBenchmark(size_t frame_id,
                                   const std::vector<base::ObjectPtr>& objects,
                                   const std::string& path) {
    std::ofstream fout(path);
    if (!fout.is_open()) {
      AERROR << "Failed to open: " << path << std::endl;
      return false;
    }
    fout << frame_id << " " << objects.size() << std::endl;
    for (auto& object : objects) {
      fout << "velodyne_64"
           << " " << object->id << " " << object->track_id << " "
           << object->lidar_supplement.is_background << " "
           << std::setprecision(8) << object->confidence << " ";

      std::string type = "unknow";
      if (object->type == base::ObjectType::UNKNOWN ||
          object->type == base::ObjectType::UNKNOWN_MOVABLE ||
          object->type == base::ObjectType::UNKNOWN_UNMOVABLE) {
        // type is unknow in this case
      } else if (object->type == base::ObjectType::PEDESTRIAN) {
        type = "pedestrian";
      } else if (object->type == base::ObjectType::VEHICLE) {
        type = "smallMot";
      } else if (object->type == base::ObjectType::BICYCLE) {
        type = "nonMot";
      }

      double yaw = atan2(object->direction[1], object->direction[0]);
      auto& object_cloud = object->lidar_supplement.cloud;

      fout << type << " " << object->center(0) << " " << object->center(1)
           << " " << object->center(2) << " " << object->size(0) << " "
           << object->size(1) << " " << object->size(2) << " "
           << yaw /*yaw*/ << " " << 0 /*roll*/ << " " << 0 /*pitch*/ << " "
           << 0 /*truncated*/ << " " << 0 /*occluded*/ << " "
           << object->velocity(0) << " " << object->velocity(1) << " "
           << object->velocity(2) << " ";
      fout << object_cloud.size() << " ";
      for (size_t i = 0; i < object_cloud.size(); ++i) {
        auto& pt = object_cloud.at(i);
        fout << std::setprecision(8) << pt.x << " " << pt.y << " " << pt.z
             << " " << pt.intensity << " ";
      }
      // fout << 0/*#indices*/ << " ";
      // we save polygon here for debug
      int polygon_size = -static_cast<int>(object->polygon.size());
      fout << polygon_size /*negative polygon size*/ << " ";
      for (size_t i = 0; i < object->polygon.size(); ++i) {
        const auto& pt = object->polygon[i];
        fout << std::setprecision(8) << pt.x << " " << pt.y << " " << pt.z
             << " ";
      }
      fout << std::endl;
    }
    // we save road boundary and road/junction polygon here for debug
    if (frame_->hdmap_struct != nullptr) {
      // transform to local frame, note since missing valid z value, the local
      // coordinates may not be accurate, only for visualization purpose
      double z = frame_->lidar2world_pose(2, 3) - 1.7;
      Eigen::Affine3d world2lidar_pose = frame_->lidar2world_pose.inverse();
      auto write_transformed_polygon = [&](const base::PolygonDType& poly) {
        Eigen::Vector3d point;
        fout << poly.size() << " ";
        for (size_t i = 0; i < poly.size(); ++i) {
          const auto& pt = poly[i];
          point << pt.x, pt.y, z;
          point = world2lidar_pose * point;
          fout << std::setprecision(8) << point(0) << " " << point(1) << " "
               << point(2) << " ";
        }
        fout << std::endl;
      };
      // 1. write road boundary
      fout << frame_->hdmap_struct->road_boundary.size() << std::endl;
      for (auto& pair : frame_->hdmap_struct->road_boundary) {
        write_transformed_polygon(pair.left_boundary);
        write_transformed_polygon(pair.right_boundary);
      }
      // 2. write road/junction polygon
      fout << frame_->hdmap_struct->road_polygons.size() +
                  frame_->hdmap_struct->junction_polygons.size()
           << std::endl;
      for (auto& poly : frame_->hdmap_struct->road_polygons) {
        write_transformed_polygon(poly);
      }
      for (auto& poly : frame_->hdmap_struct->junction_polygons) {
        write_transformed_polygon(poly);
      }
      // 3. save points label
      fout << frame_->cloud->size() << " ";
      for (size_t i = 0; i < frame_->cloud->size(); ++i) {
        fout << static_cast<int>(frame_->cloud->points_label(i)) << " ";
      }
      fout << std::endl;
    }
    fout.close();
    return true;
  }

 protected:
  std::string output_dir_;
  std::shared_ptr<LidarFrame> frame_;
  LidarObstacleSegmentationInitOptions segment_init_options_;
  LidarObstacleSegmentationOptions segment_options_;
  LidarObstacleTrackingInitOptions tracking_init_options_;
  LidarObstacleTrackingOptions tracking_options_;
  std::unique_ptr<LidarObstacleSegmentation> lidar_segmentation_;
  std::unique_ptr<LidarObstacleTracking> lidar_tracking_;
  base::SensorInfo sensor_info_;
};

}  // namespace lidar
}  // namespace perception
}  // namespace apollo

int main(int argc, char* argv[]) {
  FLAGS_alsologtostderr = 1;
  google::ParseCommandLineFlags(&argc, &argv, true);
  google::InitGoogleLogging(argv[0]);

  apollo::perception::lidar::OfflineLidarObstaclePerception test;
  if (!test.setup()) {
    AINFO << "Failed to setup OfflineLidarObstaclePerception";
    return 0;
  }
  test.run();
  return 0;
}
<|endoftext|>


<commit_before>#include <functional>

#include "test_helpers.hxx"

using namespace std;
using namespace pqxx;

// Test program for libpqxx.  Verify abort behaviour of transactor.
//
// The program will attempt to add an entry to a table called "pqxxevents",
// with a key column called "year"--and then abort the change.
//
// Note for the superstitious: the numbering for this test program is pure
// coincidence.
namespace
{

// Let's take a boring year that is not going to be in the "pqxxevents" table
const unsigned int BoringYear = 1977;

#include <pqxx/internal/ignore-deprecated-pre.hxx>

// Count events and specifically events occurring in Boring Year, leaving the
// former count in the result pair's first member, and the latter in second.
pair<int, int> count_events(connection_base &conn, string table)
{
  int all_years, boring_year;

  const string CountQuery = "SELECT count(*) FROM " + table;

  work tx{conn};
  row R;

  R = tx.exec1(CountQuery);
  R.front().to(all_years);

  R = tx.exec1(CountQuery + " WHERE year=" + to_string(BoringYear));
  R.front().to(boring_year);

  return std::make_pair(all_years, boring_year);
};

struct deliberate_error : exception
{
};

class FailedInsert : public transactor<>
{
  string m_table;
public:
  explicit FailedInsert(string Table) :
    transactor<>("FailedInsert"),
    m_table(Table)
  {
  }

  void operator()(argument_type &T)
  {
    result R = T.exec0(
	"INSERT INTO " + m_table + " VALUES (" +
	to_string(BoringYear) + ", "
	"'yawn')");

    PQXX_CHECK_EQUAL(R.affected_rows(), 1u, "Bad affected_rows().");

    throw deliberate_error();
  }

  void on_abort(const char Reason[]) noexcept
  {
    pqxx::test::expected_exception(name() + " failed: " + Reason);
  }
};

void test_013(transaction_base &T)
{
  connection_base &C{T.conn()};
  T.abort();

  {
    work T2(C);
    test::create_pqxxevents(T2);
    T2.commit();
  }

  const string Table = "pqxxevents";

  const pair<int,int> Before = perform(bind(count_events, ref(C), Table));
  PQXX_CHECK_EQUAL(
	Before.second,
	0,
	"Already have event for " + to_string(BoringYear) + "--can't test.");

  const FailedInsert DoomedTransaction(Table);
  quiet_errorhandler d(C);
  PQXX_CHECK_THROWS(
	C.perform(DoomedTransaction),
	deliberate_error,
	"Failing transactor failed to throw correct exception.");

  const pair<int,int> After = perform(bind(count_events, ref(C), Table));

  PQXX_CHECK_EQUAL(
	After.first,
	Before.first,
	"abort() didn't reset event count.");

  PQXX_CHECK_EQUAL(
	After.second,
	Before.second,
	"abort() didn't reset event count for " + to_string(BoringYear));
}

#include <pqxx/internal/ignore-deprecated-post.hxx>

} // namespace

PQXX_REGISTER_TEST_T(test_013, nontransaction)
<commit_msg>Upgrade another transactor.<commit_after>#include <functional>

#include "test_helpers.hxx"

using namespace std;
using namespace pqxx;

// Test program for libpqxx.  Verify abort behaviour of transactor.
//
// The program will attempt to add an entry to a table called "pqxxevents",
// with a key column called "year"--and then abort the change.
//
// Note for the superstitious: the numbering for this test program is pure
// coincidence.
namespace
{

// Let's take a boring year that is not going to be in the "pqxxevents" table
const unsigned int BoringYear = 1977;

// Count events and specifically events occurring in Boring Year, leaving the
// former count in the result pair's first member, and the latter in second.
pair<int, int> count_events(connection_base &conn, string table)
{
  int all_years, boring_year;

  const string CountQuery = "SELECT count(*) FROM " + table;

  work tx{conn};
  row R;

  R = tx.exec1(CountQuery);
  R.front().to(all_years);

  R = tx.exec1(CountQuery + " WHERE year=" + to_string(BoringYear));
  R.front().to(boring_year);

  return std::make_pair(all_years, boring_year);
};

struct deliberate_error : exception
{
};

void failed_insert(connection_base &C, string table)
{
  work tx(C);
  result R = tx.exec0(
	"INSERT INTO " + table + " VALUES (" +
	to_string(BoringYear) + ", "
	"'yawn')");

  PQXX_CHECK_EQUAL(R.affected_rows(), 1u, "Bad affected_rows().");
  throw deliberate_error();
}

void test_013(transaction_base &T)
{
  connection_base &C{T.conn()};
  T.abort();

  {
    work T2(C);
    test::create_pqxxevents(T2);
    T2.commit();
  }

  const string Table = "pqxxevents";

  const pair<int,int> Before = perform(bind(count_events, ref(C), Table));
  PQXX_CHECK_EQUAL(
	Before.second,
	0,
	"Already have event for " + to_string(BoringYear) + "--can't test.");

  quiet_errorhandler d(C);
  PQXX_CHECK_THROWS(
	perform(bind(failed_insert,  ref(C), Table)),
	deliberate_error,
	"Failing transactor failed to throw correct exception.");

  const pair<int,int> After = perform(bind(count_events, ref(C), Table));

  PQXX_CHECK_EQUAL(
	After.first,
	Before.first,
	"abort() didn't reset event count.");

  PQXX_CHECK_EQUAL(
	After.second,
	Before.second,
	"abort() didn't reset event count for " + to_string(BoringYear));
}
} // namespace

PQXX_REGISTER_TEST_T(test_013, nontransaction)
<|endoftext|>


<commit_before>#include "pch.h"
#include "zip_packages.h"

std::unordered_map< std::string, std::shared_ptr< zip > > ZipPackagesCache::_packages;

/**
 * Gets the fully resolved path.
 * If the path is relative then it will be prefixed with the resource path.
 * Aliases will be converted to a relative path.
 *
 * @param path The path to resolve.
 * @param fullPath The full resolved path. (out param)
 */
static void getFullPath(const char* path, std::string& fullPath)
{
    if (gameplay::FileSystem::isAbsolutePath(path))
    {
        fullPath.assign(path);
    }
    else
    {
        fullPath.assign(gameplay::FileSystem::getResourcePath());
        fullPath += gameplay::FileSystem::resolvePath(path);
    }
}

zip * ZipPackagesCache::findOrOpenPackage(const char * packageName)
{
    if (packageName == NULL || *packageName == '\0')
        return NULL;

    auto package = _packages.find(packageName);
    if (package == _packages.end())
    {
#ifndef __ANDROID__
        std::string fullPath;
        getFullPath(packageName, fullPath);
#else
        std::string fullPath(packageName);
#endif

        int err = 0;
        zip * res = zip_open(fullPath.c_str(), 0, &err);

        if (!res)
        {
            GP_WARN("Can't open package %s", packageName);
            return NULL;
        }

        _packages.insert(std::make_pair(std::string(packageName), std::shared_ptr<zip>(res, zip_close)));
        return res;
    }

    return (*package).second.get();
}

bool ZipPackagesCache::hasFile(const char * packageName, const char * filename)
{
    zip * package = findOrOpenPackage(packageName);
    if (!package)
        return gameplay::FileSystem::fileExists(filename);

    return zip_name_locate(package, filename, ZIP_FL_ENC_GUESS | ZIP_FL_NOCASE) >= 0;
}<commit_msg>return null when file can't be found inside zip package instead of fallback to FileSystem::fileExists<commit_after>#include "pch.h"
#include "zip_packages.h"

std::unordered_map< std::string, std::shared_ptr< zip > > ZipPackagesCache::_packages;

/**
 * Gets the fully resolved path.
 * If the path is relative then it will be prefixed with the resource path.
 * Aliases will be converted to a relative path.
 *
 * @param path The path to resolve.
 * @param fullPath The full resolved path. (out param)
 */
static void getFullPath(const char* path, std::string& fullPath)
{
    if (gameplay::FileSystem::isAbsolutePath(path))
    {
        fullPath.assign(path);
    }
    else
    {
        fullPath.assign(gameplay::FileSystem::getResourcePath());
        fullPath += gameplay::FileSystem::resolvePath(path);
    }
}

zip * ZipPackagesCache::findOrOpenPackage(const char * packageName)
{
    if (packageName == NULL || *packageName == '\0')
        return NULL;

    auto package = _packages.find(packageName);
    if (package == _packages.end())
    {
#ifndef __ANDROID__
        std::string fullPath;
        getFullPath(packageName, fullPath);
#else
        std::string fullPath(packageName);
#endif

        int err = 0;
        zip * res = zip_open(fullPath.c_str(), 0, &err);

        if (!res)
        {
            GP_WARN("Can't open package %s", packageName);
            return NULL;
        }

        _packages.insert(std::make_pair(std::string(packageName), std::shared_ptr<zip>(res, zip_close)));
        return res;
    }

    return (*package).second.get();
}

bool ZipPackagesCache::hasFile(const char * packageName, const char * filename)
{
    zip * package = findOrOpenPackage(packageName);
    if (!package)
        return NULL;

    return zip_name_locate(package, filename, ZIP_FL_ENC_GUESS | ZIP_FL_NOCASE) >= 0;
}<|endoftext|>


<commit_before>
#ifdef  WIN32
  #include <conio.h>
  #include <windows.h>
  #include <winbase.h>
#else
  #include <unistd.h>
  #include <fcntl.h>
  #include "myconio.h"
#endif //WIN32

#include <sstream>

#include "Simulator.h"
#include "Tools/NaoTime.h"

#include <Messages/Framework-Representations.pb.h>

#include "Tools/Math/Common.h"
#include "PlatformInterface/Platform.h"

using namespace std;
using namespace naoth;

Simulator::Simulator(const std::string& filePath, bool backendMode, bool realTime, unsigned short port)
: PlatformInterface("LogSimulator", CYCLE_TIME),
  backendMode(backendMode),
  realTime(realTime),
  logFileScanner(filePath),
  lastFrameTime(0),
  simulatedTime(0),
  simulatedFrameNumber(0),
  debugPort(port)
{
  // TODO: we need a better solution for it, but now it's the
  // fastest way to provide stuff for motion
  // register basic sensor input
  registerInput<AccelerometerData>(*this);
  registerInput<SensorJointData>(*this);
  registerInput<Image>(*this);
  registerInput<ImageTop>(*this);
  registerInput<FSRData>(*this);
  registerInput<GyrometerData>(*this);
  registerInput<InertialSensorData>(*this);
  registerInput<IRReceiveData>(*this);
  registerInput<CurrentCameraSettings>(*this);
  registerInput<ButtonData>(*this);
  registerInput<BatteryData>(*this);
  registerInput<UltraSoundReceiveData>(*this);

  registerInput<FrameInfo>(*this);

  registerInput<DebugMessageInCognition>(*this);
  registerInput<DebugMessageInMotion>(*this);
  registerOutput<DebugMessageOut>(*this);

  jumpToBegin();
}

void Simulator::open(const std::string& filePath)
{
  logFileScanner.open(filePath);
  lastFrameTime = 0;
  //simulatedTime = 0;
  //simulatedFrameNumber = 0;

  jumpToBegin();
}

void Simulator::init()
{
  lastFrameTime = 0;
  simulatedTime = 0;
  theDebugServer.start(debugPort);
  theDebugServer.setTimeOut(0);
}

void Simulator::printRepresentations()
{
  // list all representations included in the logfile
  std::cout << "-----------------------------------------" << std::endl;
  std::cout << "Representations contained in the logfile:" << std::endl;
  for(std::set<std::string>::iterator itIncluded = logFileScanner.getIncludedRepresentations().begin();
    itIncluded != logFileScanner.getIncludedRepresentations().end(); itIncluded++)
  {
    std::cout << *itIncluded << std::endl;
  }
  std::cout << "-----------------------------------------" << std::endl;
}

void Simulator::printHelp()
{
  cout << endl;
  cout << "Welcome to the NaoTH logfile based simulator" << endl;
  cout << "--------------------------------------------" << endl << endl;

  cout << "\"WASD\"-control" << endl;
  cout << "d - one step forward" << endl;
  cout << "a - one step back" << endl;
  cout << "w - to the beginning" << endl;
  cout << "s - to the end" << endl << endl;

  cout << "g - jump to specific frame" << endl;
  cout << "p - play to end (end by pressing p again)" << endl;
  cout << "l - play loop (end by pressing l again)" << endl;
  cout << "r - repeat a frame" << endl;
  cout << "q or x - quit/exit" << endl << endl;

  cout << "After a frame was executed you will always get a line showing you the current frame and the minimal and maximal frame number" << endl;
}//end printHelp

void Simulator::printCurrentLineInfo()
{
  if(logFileScanner.begin() == logFileScanner.end()) {
    cout << "The logfile seem to be empty.\t\r";
    return;
  }

  LogFileScanner::FrameIterator begin = logFileScanner.begin();
  LogFileScanner::FrameIterator end = logFileScanner.last();

  // output some informations about the current frame
  if(!backendMode) {
    cout << "[" << *currentFrame << "|" << *begin << "-" << *end << "]\t\r";
  }
}//end printCurrentLineInfo

char Simulator::getInput()
{
  if (backendMode) {
    return static_cast<char>(getchar());
  } else {
    return static_cast<char>(getch());
  }
}

void Simulator::main(bool start)
{
  init();

  printRepresentations();
  printHelp();

  jumpToBegin();

  char c = start ? 'p' : ' ';
  while(c != 'q' && c != 'x')
  {
    if(c == 'd') {
      stepForward();
    } else if(c == 'a') {
      stepBack();
    } else if(c == 'w') {
      jumpToBegin();
    } else if(c == 's') {
      jumpToEnd();
    } else if(c == 'g') {
      // read jump position
      unsigned int jpos;
      cout << " goto position: ";
      cin >> jpos;
      jumpTo(jpos);
    } else if(c == 'p') {
      play(false);
    } else if(c == 'l') {
      play(true);
    } else if(c == 'r') {
      executeCurrentFrame();
    } else if(c == 'h') {
      printHelp();
      printCurrentLineInfo();
    }
    // get new cmd char
    c = getInput();
  }// while

  cout << endl << "bye bye!" << endl;
}//end main

void Simulator::play(bool loop)
{
#ifdef WIN32
  //cerr << "Play-Support now yet enabled under Windows" << endl;
#else
  // set terminal to non-blocking...
  const int fd = fileno(stdin);
  const int fcflags = fcntl(fd,F_GETFL);
  if (fcntl(fd,F_SETFL,fcflags | O_NONBLOCK) <0)
  {
    cerr << "Could not set terminal to non-blocking mode" << endl;
    cerr << "\"Play\" capatibility not available on this terminal" << endl;
    return;
  }
#endif //WIN32

  int c = -1;
  while(c != 'l' && c != 'p' && c != '\n' && c != 's' && c != 'q' && c !='x')
  {
    // execute the frame and calculate the time to wait
    unsigned int simulatedTimeBefore = simulatedTime;
    unsigned int startTime = NaoTime::getNaoTimeInMilliSeconds();
    stepForward();
    unsigned int calculationTime = NaoTime::getNaoTimeInMilliSeconds() - startTime;
    unsigned int maxTimeToWait = realTime?simulatedTime - simulatedTimeBefore:33;

    // wait at least 5ms but max 1s
    unsigned int waitTime = Math::clamp((int)maxTimeToWait - (int)calculationTime, 5, 1000);

#ifdef WIN32
    Sleep(waitTime);
    if(_kbhit()) {
      c = getInput();
    }
#else
    // wait some time
    usleep(waitTime * 1000);
    c = getInput();
#endif

    if(!loop && currentFrame == logFileScanner.last()) {
      break;
    }
  }//while

#ifdef WIN32
  //cerr << "Play-Support now yet enabled under Windows" << endl;
#else
  // set back to blocking
  if (fcntl(fd,F_SETFL,fcflags) <0)
  {
    cerr << "Could not set terminal to blocking mode" << endl;
    cerr << "terminating since this is a serious error" << endl;
    exit(EXIT_FAILURE);
  }
#endif //WIN32
}//end play

void Simulator::stepForward()
{
  currentFrame++;
  if(currentFrame == logFileScanner.end()) {
    jumpToBegin();
  } else {
    executeCurrentFrame();
  }
}

void Simulator::stepBack()
{
  if(currentFrame == logFileScanner.begin()) {
    jumpToEnd();
  } else {
    currentFrame--;
    executeCurrentFrame();
  }
}

void Simulator::jumpToBegin()
{
  currentFrame = logFileScanner.begin();
  executeCurrentFrame();
}

void Simulator::jumpToEnd()
{
  currentFrame = logFileScanner.end();
  currentFrame--;
  executeCurrentFrame();
}

void Simulator::jumpTo(unsigned int position)
{
  // do it the stupid way...
  bool wasFound = false;
  LogFileScanner::FrameIterator oldPos = currentFrame;
  currentFrame = logFileScanner.begin();
  while(!wasFound && currentFrame != logFileScanner.end())
  {
    if(*currentFrame == position) {
      wasFound = true;
      break;
    } else {
      currentFrame++;
    }
  }//end while

  if(wasFound) {
    executeCurrentFrame();
  } else {
    cout << "frame not found!" << endl;
    currentFrame = oldPos;
    if(!backendMode) { printCurrentLineInfo(); }
  }
}//end jumpTo

void Simulator::executeCurrentFrame()
{
  logFileScanner.readFrame(*currentFrame, representations);

  //
  adjust_frame_time();

  // execute
  runCognition();
  runMotion();

  //std::cout << "end executeCurrentFrame" << std::endl;

  printCurrentLineInfo();

}//end executeCurrentFrame

void Simulator::adjust_frame_time()
{
  naothmessages::FrameInfo f;

  // default time since the last frame
  int time_delta = 33; // default time step simulating 30fps

  // if no FrameInfo was logged set it manually
  LogFileScanner::RepresentationData& frameData = representations["FrameInfo"];
  if(!frameData.valid)
  {
    f.set_framenumber(*currentFrame);
    lastFrameTime = 0;
  }
  else
  {
    // read the actual frame info
    f.ParseFromArray(frameData.data.data(), (int) frameData.data.size());
    unsigned int frameTime = f.time();

    // logged time since the last frame
    if(lastFrameTime != 0) {
      time_delta = (int)frameTime - (int)lastFrameTime;
    }

    // remember the current time for next cycle
    lastFrameTime = frameTime;
  }

  if(time_delta <= 0) {
    time_delta = 33; // default time step simulating 30fps
  }

  simulatedTime += time_delta;
  f.set_time(simulatedTime);

  f.set_framenumber(simulatedFrameNumber++);

  // write the result back
  string result = f.SerializeAsString();
  frameData.data.resize(result.size());
  std::copy ( result.begin(), result.end(), frameData.data.begin() );
}//end adjust_frame_time

///// Getter/Setter /////

template<class T>
void Simulator::generalGet(T& data, std::string name) const
{
  LogFileScanner::Frame::const_iterator iter = representations.find(name);
  if(iter != representations.end() && iter->second.valid)
  {
    std::istrstream stream(iter->second.data.data(), iter->second.data.size());
    Serializer<T>::deserialize(stream, data);
  }
}//end generalGet

///// end Getter/Setter /////

MessageQueue* Simulator::createMessageQueue(const std::string& /*name*/)
{
  // for single thread
  return new MessageQueue();
}
<commit_msg>autoplay parameter for logfilesimulator now exits after logfile was finished playing<commit_after>
#ifdef  WIN32
  #include <conio.h>
  #include <windows.h>
  #include <winbase.h>
#else
  #include <unistd.h>
  #include <fcntl.h>
  #include "myconio.h"
#endif //WIN32

#include <sstream>

#include "Simulator.h"
#include "Tools/NaoTime.h"

#include <Messages/Framework-Representations.pb.h>

#include "Tools/Math/Common.h"
#include "PlatformInterface/Platform.h"

using namespace std;
using namespace naoth;

Simulator::Simulator(const std::string& filePath, bool backendMode, bool realTime, unsigned short port)
: PlatformInterface("LogSimulator", CYCLE_TIME),
  backendMode(backendMode),
  realTime(realTime),
  logFileScanner(filePath),
  lastFrameTime(0),
  simulatedTime(0),
  simulatedFrameNumber(0),
  debugPort(port)
{
  // TODO: we need a better solution for it, but now it's the
  // fastest way to provide stuff for motion
  // register basic sensor input
  registerInput<AccelerometerData>(*this);
  registerInput<SensorJointData>(*this);
  registerInput<Image>(*this);
  registerInput<ImageTop>(*this);
  registerInput<FSRData>(*this);
  registerInput<GyrometerData>(*this);
  registerInput<InertialSensorData>(*this);
  registerInput<IRReceiveData>(*this);
  registerInput<CurrentCameraSettings>(*this);
  registerInput<ButtonData>(*this);
  registerInput<BatteryData>(*this);
  registerInput<UltraSoundReceiveData>(*this);

  registerInput<FrameInfo>(*this);

  registerInput<DebugMessageInCognition>(*this);
  registerInput<DebugMessageInMotion>(*this);
  registerOutput<DebugMessageOut>(*this);

  jumpToBegin();
}

void Simulator::open(const std::string& filePath)
{
  logFileScanner.open(filePath);
  lastFrameTime = 0;
  //simulatedTime = 0;
  //simulatedFrameNumber = 0;

  jumpToBegin();
}

void Simulator::init()
{
  lastFrameTime = 0;
  simulatedTime = 0;
  theDebugServer.start(debugPort);
  theDebugServer.setTimeOut(0);
}

void Simulator::printRepresentations()
{
  // list all representations included in the logfile
  std::cout << "-----------------------------------------" << std::endl;
  std::cout << "Representations contained in the logfile:" << std::endl;
  for(std::set<std::string>::iterator itIncluded = logFileScanner.getIncludedRepresentations().begin();
    itIncluded != logFileScanner.getIncludedRepresentations().end(); itIncluded++)
  {
    std::cout << *itIncluded << std::endl;
  }
  std::cout << "-----------------------------------------" << std::endl;
}

void Simulator::printHelp()
{
  cout << endl;
  cout << "Welcome to the NaoTH logfile based simulator" << endl;
  cout << "--------------------------------------------" << endl << endl;

  cout << "\"WASD\"-control" << endl;
  cout << "d - one step forward" << endl;
  cout << "a - one step back" << endl;
  cout << "w - to the beginning" << endl;
  cout << "s - to the end" << endl << endl;

  cout << "g - jump to specific frame" << endl;
  cout << "p - play to end (end by pressing p again)" << endl;
  cout << "l - play loop (end by pressing l again)" << endl;
  cout << "r - repeat a frame" << endl;
  cout << "q or x - quit/exit" << endl << endl;

  cout << "After a frame was executed you will always get a line showing you the current frame and the minimal and maximal frame number" << endl;
}//end printHelp

void Simulator::printCurrentLineInfo()
{
  if(logFileScanner.begin() == logFileScanner.end()) {
    cout << "The logfile seem to be empty.\t\r";
    return;
  }

  LogFileScanner::FrameIterator begin = logFileScanner.begin();
  LogFileScanner::FrameIterator end = logFileScanner.last();

  // output some informations about the current frame
  if(!backendMode) {
    cout << "[" << *currentFrame << "|" << *begin << "-" << *end << "]\t\r";
  }
}//end printCurrentLineInfo

char Simulator::getInput()
{
  if (backendMode) {
    return static_cast<char>(getchar());
  } else {
    return static_cast<char>(getch());
  }
}

void Simulator::main(bool autostart)
{
  init();

  printRepresentations();
  printHelp();

  jumpToBegin();
	if (autostart){
		play(false);
	}
	else{
		char c;
		while ((c = getInput()) && c != 'q' && c != 'x')
		{
			if (c == 'd') {
				stepForward();
			}
			else if (c == 'a') {
				stepBack();
			}
			else if (c == 'w') {
				jumpToBegin();
			}
			else if (c == 's') {
				jumpToEnd();
			}
			else if (c == 'g') {
				// read jump position
				unsigned int jpos;
				cout << " goto position: ";
				cin >> jpos;
				jumpTo(jpos);
			}
			else if (c == 'p') {
				play(false);
			}
			else if (c == 'l') {
				play(true);
			}
			else if (c == 'r') {
				executeCurrentFrame();
			}
			else if (c == 'h') {
				printHelp();
				printCurrentLineInfo();
			}
		}// while
	}

  cout << endl << "bye bye!" << endl;
}//end main

void Simulator::play(bool loop)
{
#ifdef WIN32
  //cerr << "Play-Support now yet enabled under Windows" << endl;
#else
  // set terminal to non-blocking...
  const int fd = fileno(stdin);
  const int fcflags = fcntl(fd,F_GETFL);
  if (fcntl(fd,F_SETFL,fcflags | O_NONBLOCK) <0)
  {
    cerr << "Could not set terminal to non-blocking mode" << endl;
    cerr << "\"Play\" capatibility not available on this terminal" << endl;
    return;
  }
#endif //WIN32

  int c = -1;
  while(c != 'l' && c != 'p' && c != '\n' && c != 's' && c != 'q' && c !='x')
  {
    // execute the frame and calculate the time to wait
    unsigned int simulatedTimeBefore = simulatedTime;
    unsigned int startTime = NaoTime::getNaoTimeInMilliSeconds();
    stepForward();
    unsigned int calculationTime = NaoTime::getNaoTimeInMilliSeconds() - startTime;
    unsigned int maxTimeToWait = realTime?simulatedTime - simulatedTimeBefore:33;

    // wait at least 5ms but max 1s
    unsigned int waitTime = Math::clamp((int)maxTimeToWait - (int)calculationTime, 5, 1000);

#ifdef WIN32
    Sleep(waitTime);
    if(_kbhit()) {
      c = getInput();
    }
#else
    // wait some time
    usleep(waitTime * 1000);
    c = getInput();
#endif

    if(!loop && currentFrame == logFileScanner.last()) {
      break;
    }
  }//while

#ifdef WIN32
  //cerr << "Play-Support now yet enabled under Windows" << endl;
#else
  // set back to blocking
  if (fcntl(fd,F_SETFL,fcflags) <0)
  {
    cerr << "Could not set terminal to blocking mode" << endl;
    cerr << "terminating since this is a serious error" << endl;
    exit(EXIT_FAILURE);
  }
#endif //WIN32
}//end play

void Simulator::stepForward()
{
  currentFrame++;
  if(currentFrame == logFileScanner.end()) {
    jumpToBegin();
  } else {
    executeCurrentFrame();
  }
}

void Simulator::stepBack()
{
  if(currentFrame == logFileScanner.begin()) {
    jumpToEnd();
  } else {
    currentFrame--;
    executeCurrentFrame();
  }
}

void Simulator::jumpToBegin()
{
  currentFrame = logFileScanner.begin();
  executeCurrentFrame();
}

void Simulator::jumpToEnd()
{
  currentFrame = logFileScanner.end();
  currentFrame--;
  executeCurrentFrame();
}

void Simulator::jumpTo(unsigned int position)
{
  // do it the stupid way...
  bool wasFound = false;
  LogFileScanner::FrameIterator oldPos = currentFrame;
  currentFrame = logFileScanner.begin();
  while(!wasFound && currentFrame != logFileScanner.end())
  {
    if(*currentFrame == position) {
      wasFound = true;
      break;
    } else {
      currentFrame++;
    }
  }//end while

  if(wasFound) {
    executeCurrentFrame();
  } else {
    cout << "frame not found!" << endl;
    currentFrame = oldPos;
    if(!backendMode) { printCurrentLineInfo(); }
  }
}//end jumpTo

void Simulator::executeCurrentFrame()
{
  logFileScanner.readFrame(*currentFrame, representations);

  //
  adjust_frame_time();

  // execute
  runCognition();
  runMotion();

  //std::cout << "end executeCurrentFrame" << std::endl;

  printCurrentLineInfo();

}//end executeCurrentFrame

void Simulator::adjust_frame_time()
{
  naothmessages::FrameInfo f;

  // default time since the last frame
  int time_delta = 33; // default time step simulating 30fps

  // if no FrameInfo was logged set it manually
  LogFileScanner::RepresentationData& frameData = representations["FrameInfo"];
  if(!frameData.valid)
  {
    f.set_framenumber(*currentFrame);
    lastFrameTime = 0;
  }
  else
  {
    // read the actual frame info
    f.ParseFromArray(frameData.data.data(), (int) frameData.data.size());
    unsigned int frameTime = f.time();

    // logged time since the last frame
    if(lastFrameTime != 0) {
      time_delta = (int)frameTime - (int)lastFrameTime;
    }

    // remember the current time for next cycle
    lastFrameTime = frameTime;
  }

  if(time_delta <= 0) {
    time_delta = 33; // default time step simulating 30fps
  }

  simulatedTime += time_delta;
  f.set_time(simulatedTime);

  f.set_framenumber(simulatedFrameNumber++);

  // write the result back
  string result = f.SerializeAsString();
  frameData.data.resize(result.size());
  std::copy ( result.begin(), result.end(), frameData.data.begin() );
}//end adjust_frame_time

///// Getter/Setter /////

template<class T>
void Simulator::generalGet(T& data, std::string name) const
{
  LogFileScanner::Frame::const_iterator iter = representations.find(name);
  if(iter != representations.end() && iter->second.valid)
  {
    std::istrstream stream(iter->second.data.data(), iter->second.data.size());
    Serializer<T>::deserialize(stream, data);
  }
}//end generalGet

///// end Getter/Setter /////

MessageQueue* Simulator::createMessageQueue(const std::string& /*name*/)
{
  // for single thread
  return new MessageQueue();
}
<|endoftext|>


<commit_before>//=======================================================================
// Copyright Baptiste Wicht 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include "il/Math.hpp"
#include "il/Operand.hpp"
#include "AssemblyFileWriter.hpp"

using namespace eddic;

Math::Math(Operation operation, std::shared_ptr<Operand> lhs, std::shared_ptr<Operand> rhs) : m_operation(operation), m_lhs(lhs), m_rhs(rhs) {}

void Math::write(AssemblyFileWriter& writer){
    //TODO Improve ?
    switch(m_operation){
        case Operation::ADD:
            writer.stream() << "addl " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
        case Operation::SUB:
            writer.stream() << "subl " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
        case Operation::DIV:
            writer.stream() << "divl " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
        case Operation::MUL:
            writer.stream() << "mull " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
        case Operation::MOD:
            writer.stream() << "modl " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
    }
}
<commit_msg>Solve math operations<commit_after>//=======================================================================
// Copyright Baptiste Wicht 2011.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include "il/Math.hpp"
#include "il/Operand.hpp"
#include "AssemblyFileWriter.hpp"

using namespace eddic;

Math::Math(Operation operation, std::shared_ptr<Operand> lhs, std::shared_ptr<Operand> rhs) : m_operation(operation), m_lhs(lhs), m_rhs(rhs) {}

void Math::write(AssemblyFileWriter& writer){
    //TODO Warn if eax or edx is used by the operands

    //TODO Improve ?
    switch(m_operation){
        case Operation::ADD:
            writer.stream() << "addl " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
        case Operation::SUB:
            writer.stream() << "subl " << m_lhs->getValue() << ", " << m_rhs->getValue() << std::endl;

            break;
        case Operation::DIV:
            writer.stream() << "movl " << m_lhs->getValue() << ", %eax" << std::endl;
            writer.stream() << "movl " << m_rhs->getValue() << ", %ecx" << std::endl;
            writer.stream() << "movl $0, %edx" << std::endl;
            writer.stream() << "divl %ecx" << std::endl;
            writer.stream() << "movl %eax, " << m_rhs->getValue() << std::endl;

            break;
        case Operation::MUL:
            writer.stream() << "movl " << m_lhs->getValue() << ", %eax" << std::endl;
            writer.stream() << "movl " << m_rhs->getValue() << ", %ecx" << std::endl;
            writer.stream() << "mull %ecx" << std::endl;
            writer.stream() << "movl %eax, " << m_rhs->getValue() << std::endl;

            break;
        case Operation::MOD:
            writer.stream() << "movl " << m_lhs->getValue() << ", %eax" << std::endl;
            writer.stream() << "movl " << m_rhs->getValue() << ", %ecx" << std::endl;
            writer.stream() << "movl $0, %edx" << std::endl;
            writer.stream() << "divl %ecx" << std::endl;
            writer.stream() << "movl %edx, " << m_rhs->getValue() << std::endl;

            break;
    }
}
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkQtChartColorStyleGenerator.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/

/// \file vtkQtChartColorStyleGenerator.cxx
/// \date September 22, 2008

#include "vtkQtChartColorStyleGenerator.h"

#include <QVector>

class vtkQtChartColorStyleGeneratorInternal
{
public:
  vtkQtChartColorStyleGeneratorInternal();
  ~vtkQtChartColorStyleGeneratorInternal() {}

  QVector<Qt::PenStyle> Styles;
};

//-----------------------------------------------------------------------------
vtkQtChartColorStyleGeneratorInternal::vtkQtChartColorStyleGeneratorInternal()
  : Styles()
{
}

//-----------------------------------------------------------------------------
vtkQtChartColorStyleGenerator::vtkQtChartColorStyleGenerator(
    QObject *parentObject, vtkQtChartColors::ColorScheme scheme)
  : vtkQtChartStyleGenerator(parentObject)
{
  this->Internal = new vtkQtChartColorStyleGeneratorInternal();
  this->Colors = new vtkQtChartColors(scheme);

  // Add the default list of pen styles.
  this->Internal->Styles.append(Qt::SolidLine);
  this->Internal->Styles.append(Qt::DashLine);
  this->Internal->Styles.append(Qt::DotLine);
  this->Internal->Styles.append(Qt::DashDotLine);
  this->Internal->Styles.append(Qt::DashDotDotLine);
}

vtkQtChartColorStyleGenerator::~vtkQtChartColorStyleGenerator()
{
  delete this->Internal;
  delete this->Colors;
}

QBrush vtkQtChartColorStyleGenerator::getSeriesBrush(int index) const
{
  if(index >= 0)
    {
    index = index % this->Colors->getNumberOfColors();
    return QBrush(this->Colors->getColor(index));
    }

  return QBrush();
}

QPen vtkQtChartColorStyleGenerator::getSeriesPen(int index) const
{
  QPen pen;
  if(index >= 0)
    {
    int numColors = this->Colors->getNumberOfColors();
    if(numColors > 0)
      {
      pen.setColor(this->Colors->getColor(index % numColors));
      index /= numColors;
      }

    int numStyles = this->Internal->Styles.size();
    if(numStyles > 0)
      {
      pen.setStyle(this->Internal->Styles[index % numStyles]);
      }
    }

  return pen;
}

int vtkQtChartColorStyleGenerator::getNumberOfStyles() const
{
  return this->Internal->Styles.size();
}

Qt::PenStyle vtkQtChartColorStyleGenerator::getPenStyle(int index) const
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    return this->Internal->Styles[index];
    }

  return Qt::SolidLine;
}

void vtkQtChartColorStyleGenerator::setPenStyle(int index, Qt::PenStyle style)
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    this->Internal->Styles[index] = style;
    }
}

void vtkQtChartColorStyleGenerator::clearPenStyles()
{
  this->Internal->Styles.clear();
}

void vtkQtChartColorStyleGenerator::addPenStyle(Qt::PenStyle style)
{
  this->Internal->Styles.append(style);
}

void vtkQtChartColorStyleGenerator::insertPenStyle(int index,
    Qt::PenStyle style)
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    this->Internal->Styles.insert(index, style);
    }
}

void vtkQtChartColorStyleGenerator::removePenStyle(int index)
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    this->Internal->Styles.remove(index);
    }
}

<commit_msg>BUG: Check for divide-by-zero case.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkQtChartColorStyleGenerator.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/
/*-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------*/

/// \file vtkQtChartColorStyleGenerator.cxx
/// \date September 22, 2008

#include "vtkQtChartColorStyleGenerator.h"

#include <QVector>

class vtkQtChartColorStyleGeneratorInternal
{
public:
  vtkQtChartColorStyleGeneratorInternal();
  ~vtkQtChartColorStyleGeneratorInternal() {}

  QVector<Qt::PenStyle> Styles;
};

//-----------------------------------------------------------------------------
vtkQtChartColorStyleGeneratorInternal::vtkQtChartColorStyleGeneratorInternal()
  : Styles()
{
}

//-----------------------------------------------------------------------------
vtkQtChartColorStyleGenerator::vtkQtChartColorStyleGenerator(
    QObject *parentObject, vtkQtChartColors::ColorScheme scheme)
  : vtkQtChartStyleGenerator(parentObject)
{
  this->Internal = new vtkQtChartColorStyleGeneratorInternal();
  this->Colors = new vtkQtChartColors(scheme);

  // Add the default list of pen styles.
  this->Internal->Styles.append(Qt::SolidLine);
  this->Internal->Styles.append(Qt::DashLine);
  this->Internal->Styles.append(Qt::DotLine);
  this->Internal->Styles.append(Qt::DashDotLine);
  this->Internal->Styles.append(Qt::DashDotDotLine);
}

vtkQtChartColorStyleGenerator::~vtkQtChartColorStyleGenerator()
{
  delete this->Internal;
  delete this->Colors;
}

QBrush vtkQtChartColorStyleGenerator::getSeriesBrush(int index) const
{
  if(index >= 0 && this->Colors->getNumberOfColors()>0)
    {
    index = index % this->Colors->getNumberOfColors();
    return QBrush(this->Colors->getColor(index));
    }

  return QBrush();
}

QPen vtkQtChartColorStyleGenerator::getSeriesPen(int index) const
{
  QPen pen;
  if(index >= 0)
    {
    int numColors = this->Colors->getNumberOfColors();
    if(numColors > 0)
      {
      pen.setColor(this->Colors->getColor(index % numColors));
      index /= numColors;
      }

    int numStyles = this->Internal->Styles.size();
    if(numStyles > 0)
      {
      pen.setStyle(this->Internal->Styles[index % numStyles]);
      }
    }

  return pen;
}

int vtkQtChartColorStyleGenerator::getNumberOfStyles() const
{
  return this->Internal->Styles.size();
}

Qt::PenStyle vtkQtChartColorStyleGenerator::getPenStyle(int index) const
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    return this->Internal->Styles[index];
    }

  return Qt::SolidLine;
}

void vtkQtChartColorStyleGenerator::setPenStyle(int index, Qt::PenStyle style)
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    this->Internal->Styles[index] = style;
    }
}

void vtkQtChartColorStyleGenerator::clearPenStyles()
{
  this->Internal->Styles.clear();
}

void vtkQtChartColorStyleGenerator::addPenStyle(Qt::PenStyle style)
{
  this->Internal->Styles.append(style);
}

void vtkQtChartColorStyleGenerator::insertPenStyle(int index,
    Qt::PenStyle style)
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    this->Internal->Styles.insert(index, style);
    }
}

void vtkQtChartColorStyleGenerator::removePenStyle(int index)
{
  if(index >= 0 && index < this->Internal->Styles.size())
    {
    this->Internal->Styles.remove(index);
    }
}

<|endoftext|>


<commit_before>/*
*  Copyright 2014 Matthias Fuchs
*
*  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 "stromx/raspi/impl/Gpio.h"

#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <boost/graph/graph_concepts.hpp>

#define LOW  0
#define HIGH 1

namespace stromx
{
namespace raspi
{
namespace impl
{

int GPIOExport(int pin)
{
#define BUFFER_MAX 3
    char buffer[BUFFER_MAX];
    ssize_t bytes_written;
    int fd;

    fd = open("/sys/class/gpio/export", O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open export for writing!\n");
        return(-1);
    }

    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
    write(fd, buffer, bytes_written);
    close(fd);
    return(0);
}

int GPIOUnexport(int pin)
{
    char buffer[BUFFER_MAX];
    ssize_t bytes_written;
    int fd;

    fd = open("/sys/class/gpio/unexport", O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open unexport for writing!\n");
        return(-1);
    }

    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
    write(fd, buffer, bytes_written);
    close(fd);
    return(0);
}

int GPIODirection(int pin, int dir)
{
    static const char s_directions_str[]  = "in\0out";

#define DIRECTION_MAX 35
    char path[DIRECTION_MAX];
    int fd;

    snprintf(path, DIRECTION_MAX, "/sys/class/gpio/gpio%d/direction", pin);
    fd = open(path, O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio direction for writing!\n");
        return(-1);
    }

    if (-1 == write(fd, &s_directions_str[IN == dir ? 0 : 3], IN == dir ? 2 : 3))
    {
        fprintf(stderr, "Failed to set direction!\n");
        return(-1);
    }

    close(fd);
    return(0);
}

int GPIOEdge(int pin, int edge)
{
    static const char s_edges_str[]  = "rising\0falling\0both";

#define EDGE_MAX 30
    char path[EDGE_MAX];
    int fd;

    snprintf(path, EDGE_MAX, "/sys/class/gpio/gpio%d/edge", pin);
    fd = open(path, O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio edge for writing!\n");
        return(-1);
    }

    const char* edge_str = 0;
    int str_length = 0;
    switch(edge)
    {
        case RISING:
            edge_str = &s_edges_str[0];
            str_length = 6;
            break;
        case FALLING:
            edge_str = &s_edges_str[7];
            str_length = 7;
            break;
        case BOTH:
            edge_str = &s_edges_str[15];
            str_length = 4;
            break;
        default:
            fprintf(stderr, "Unknown edge flag\n");
            return(-1);
    }

    if (-1 == write(fd, edge_str, str_length))
    {
        fprintf(stderr, "Failed to set edge!\n");
        return(-1);
    }

    close(fd);
    return(0);
}

int GPIORead(int pin)
{
#define VALUE_MAX 30
    char path[VALUE_MAX];
    char value_str[3];
    int fd;

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_RDONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio value for reading!\n");
        return(-1);
    }

    if (-1 == read(fd, value_str, 3))
    {
        fprintf(stderr, "Failed to read value!\n");
        return(-1);
    }

    close(fd);

    return(atoi(value_str));
}

int GPIOWrite(int pin, int value)
{
    static const char s_values_str[] = "01";

    char path[VALUE_MAX];
    int fd;

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio value for writing!\n");
        return(-1);
    }

    if (1 != write(fd, &s_values_str[LOW == value ? 0 : 1], 1))
    {
        fprintf(stderr, "Failed to write value!\n");
        return(-1);
    }

    close(fd);
    return(0);
}

int GPIOOpen(int pin, int& socket)
{
    char path[VALUE_MAX];
    int fd;
    char value_str[3];

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_RDONLY| O_NONBLOCK);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio value for reading!\n");
        return(-1);
    }

    // read the current value to make sure the first call to poll() does not
    // return immediately
    if (-1 == read(fd, value_str, 3))
    {
        fprintf(stderr, "Failed to read value!\n");
        return(-1);
    }

    socket = fd;
    return(0);
}

int GPIOCreatePipe(int& readEnd, int& writeEnd)
{
    int pipefd[2] = {0, 0};
    int rc;
    rc = pipe(pipefd);

    if (-1 == rc)
    {
        fprintf(stderr, "Failed to create pipe!\n");
        return(-1);
    }

    readEnd = pipefd[0];
    writeEnd = pipefd[1];
    return(0);
}

int GPIOPoll(int gpio, int readEnd, bool & interrupt)
{
#define MAX_BUF 64
    struct pollfd fdset[2];
    int nfds = 2;
    int rc;
    char buf[MAX_BUF];
    interrupt = false;

    memset((void*)fdset, 0, sizeof(fdset));

    fdset[0].fd = readEnd;
    fdset[0].events = POLLIN;

    fdset[1].fd = gpio;
    fdset[1].events = POLLPRI;

    rc = poll(fdset, nfds, -1);

    if (rc < 0)
    {
        fprintf(stderr, "Failed to poll!\n");
        return(-1);
    }

    if (fdset[1].revents & POLLPRI)
    {
        fprintf(stdout, "GPIO interrupt!\n");
        if (-1 == read(fdset[1].fd, buf, MAX_BUF))
        {
            fprintf(stderr, "Failed to read GPIO value!\n");
            return(-1);
        }
    }

    if (fdset[0].revents & POLLIN)
    {
        interrupt = true;
        fprintf(stdout, "Pipe interrupt!\n");
        if (-1 == read(fdset[0].fd, buf, 1))
        {
            fprintf(stderr, "Failed to read pipe value!\n");
            return(-1);
        }
    }

    return(atoi(buf));
}

int GPIOCloseSocket(int socket)
{
    if (close(socket))
    {
        fprintf(stderr, "Failed to close socket!\n");
    }

    return(0);
}

int GPIOSendInterrupt(int writeEnd)
{
    if (write(writeEnd, "\n", 1) != 1)
    {
        fprintf(stderr, "Failed to send interrupt!\n");
    }

    return(0);
}

}
}
}
<commit_msg>Set strings in gpio utilities to empty<commit_after>/*
*  Copyright 2014 Matthias Fuchs
*
*  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 "stromx/raspi/impl/Gpio.h"

#include <fcntl.h>
#include <poll.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <boost/graph/graph_concepts.hpp>

#define LOW  0
#define HIGH 1

namespace stromx
{
namespace raspi
{
namespace impl
{

int GPIOExport(int pin)
{
#define BUFFER_MAX 3
    char buffer[BUFFER_MAX];
    ssize_t bytes_written;
    int fd;

    fd = open("/sys/class/gpio/export", O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open export for writing!\n");
        return(-1);
    }

    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
    write(fd, buffer, bytes_written);
    close(fd);
    return(0);
}

int GPIOUnexport(int pin)
{
    char buffer[BUFFER_MAX];
    ssize_t bytes_written;
    int fd;

    fd = open("/sys/class/gpio/unexport", O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open unexport for writing!\n");
        return(-1);
    }

    bytes_written = snprintf(buffer, BUFFER_MAX, "%d", pin);
    write(fd, buffer, bytes_written);
    close(fd);
    return(0);
}

int GPIODirection(int pin, int dir)
{
    static const char s_directions_str[]  = "in\0out";

#define DIRECTION_MAX 35
    char path[DIRECTION_MAX];
    int fd;

    snprintf(path, DIRECTION_MAX, "/sys/class/gpio/gpio%d/direction", pin);
    fd = open(path, O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio direction for writing!\n");
        return(-1);
    }

    if (-1 == write(fd, &s_directions_str[IN == dir ? 0 : 3], IN == dir ? 2 : 3))
    {
        fprintf(stderr, "Failed to set direction!\n");
        return(-1);
    }

    close(fd);
    return(0);
}

int GPIOEdge(int pin, int edge)
{
    static const char s_edges_str[]  = "rising\0falling\0both";

#define EDGE_MAX 30
    char path[EDGE_MAX];
    int fd;

    snprintf(path, EDGE_MAX, "/sys/class/gpio/gpio%d/edge", pin);
    fd = open(path, O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio edge for writing!\n");
        return(-1);
    }

    const char* edge_str = 0;
    int str_length = 0;
    switch(edge)
    {
        case RISING:
            edge_str = &s_edges_str[0];
            str_length = 6;
            break;
        case FALLING:
            edge_str = &s_edges_str[7];
            str_length = 7;
            break;
        case BOTH:
            edge_str = &s_edges_str[15];
            str_length = 4;
            break;
        default:
            fprintf(stderr, "Unknown edge flag\n");
            return(-1);
    }

    if (-1 == write(fd, edge_str, str_length))
    {
        fprintf(stderr, "Failed to set edge!\n");
        return(-1);
    }

    close(fd);
    return(0);
}

int GPIORead(int pin)
{
#define VALUE_MAX 30
    char path[VALUE_MAX];
    char value_str[3] = "";
    int fd;

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_RDONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio value for reading!\n");
        return(-1);
    }

    if (-1 == read(fd, value_str, 3))
    {
        fprintf(stderr, "Failed to read value!\n");
        return(-1);
    }

    close(fd);

    return(atoi(value_str));
}

int GPIOWrite(int pin, int value)
{
    static const char s_values_str[] = "01";

    char path[VALUE_MAX];
    int fd;

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_WRONLY);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio value for writing!\n");
        return(-1);
    }

    if (1 != write(fd, &s_values_str[LOW == value ? 0 : 1], 1))
    {
        fprintf(stderr, "Failed to write value!\n");
        return(-1);
    }

    close(fd);
    return(0);
}

int GPIOOpen(int pin, int& socket)
{
    char path[VALUE_MAX];
    int fd;
    char value_str[3] = "";

    snprintf(path, VALUE_MAX, "/sys/class/gpio/gpio%d/value", pin);
    fd = open(path, O_RDONLY| O_NONBLOCK);
    if (-1 == fd)
    {
        fprintf(stderr, "Failed to open gpio value for reading!\n");
        return(-1);
    }

    // read the current value to make sure the first call to poll() does not
    // return immediately
    if (-1 == read(fd, value_str, 3))
    {
        fprintf(stderr, "Failed to read value!\n");
        return(-1);
    }

    socket = fd;
    return(0);
}

int GPIOCreatePipe(int& readEnd, int& writeEnd)
{
    int pipefd[2] = {0, 0};
    int rc;
    rc = pipe(pipefd);

    if (-1 == rc)
    {
        fprintf(stderr, "Failed to create pipe!\n");
        return(-1);
    }

    readEnd = pipefd[0];
    writeEnd = pipefd[1];
    return(0);
}

int GPIOPoll(int gpio, int readEnd, bool & interrupt)
{
#define MAX_BUF 64
    struct pollfd fdset[2];
    int nfds = 2;
    int rc;
    char buf[MAX_BUF] = "";
    interrupt = false;

    memset((void*)fdset, 0, sizeof(fdset));

    fdset[0].fd = readEnd;
    fdset[0].events = POLLIN;

    fdset[1].fd = gpio;
    fdset[1].events = POLLPRI;

    rc = poll(fdset, nfds, -1);

    if (rc < 0)
    {
        fprintf(stderr, "Failed to poll!\n");
        return(-1);
    }

    if (fdset[1].revents & POLLPRI)
    {
        fprintf(stdout, "GPIO interrupt!\n");
        if (-1 == read(fdset[1].fd, buf, MAX_BUF))
        {
            fprintf(stderr, "Failed to read GPIO value!\n");
            return(-1);
        }
    }

    if (fdset[0].revents & POLLIN)
    {
        interrupt = true;
        fprintf(stdout, "Pipe interrupt!\n");
        if (-1 == read(fdset[0].fd, buf, 1))
        {
            fprintf(stderr, "Failed to read pipe value!\n");
            return(-1);
        }
    }

    return(atoi(buf));
}

int GPIOCloseSocket(int socket)
{
    if (close(socket))
    {
        fprintf(stderr, "Failed to close socket!\n");
    }

    return(0);
}

int GPIOSendInterrupt(int writeEnd)
{
    if (write(writeEnd, "\n", 1) != 1)
    {
        fprintf(stderr, "Failed to send interrupt!\n");
    }

    return(0);
}

}
}
}
<|endoftext|>