<commit_before>#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>

typedef pcl::PointXYZ Point;

void
PointCloud2Vector3d (pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
{
  for (unsigned i = 0; i < cloud->size (); i++)
  {
    Point &p = cloud->at (i);
    if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z))
      data.push_back (Eigen::Vector3d (p.x, p.y, p.z));
  }
}

void
visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

  pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4);
  for (std::size_t i = 0; i < curve_cloud->size () - 1; i++)
  {
    pcl::PointXYZRGB &p1 = curve_cloud->at (i);
    pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1);
    std::ostringstream os;
    os << "line" << i;
    viewer.removeShape (os.str ());
    viewer.addLine<pcl::PointXYZRGB> (p1, p2, 1.0, 0.0, 0.0, os.str ());
  }

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps (new pcl::PointCloud<pcl::PointXYZRGB>);
  for (int i = 0; i < curve.CVCount (); i++)
  {
    ON_3dPoint p1;
    curve.GetCV (i, p1);

    double pnt[3];
    surface.Evaluate (p1.x, p1.y, 0, 3, pnt);
    pcl::PointXYZRGB p2;
    p2.x = float (pnt[0]);
    p2.y = float (pnt[1]);
    p2.z = float (pnt[2]);

    p2.r = 255;
    p2.g = 0;
    p2.b = 0;

    curve_cps->push_back (p2);
  }
  viewer.removePointCloud ("cloud_cps");
  viewer.addPointCloud (curve_cps, "cloud_cps");
}

int
main (int argc, char *argv[])
{
  std::string pcd_file;

  if (argc < 2)
  {
    printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd<PointXYZ>-file\n\n");
    exit (0);
  }

  pcd_file = argv[1];

  unsigned order (3);
  unsigned refinement (6);
  unsigned iterations (10);
  unsigned mesh_resolution (256);

  pcl::visualization::PCLVisualizer viewer ("Test: NURBS surface fitting");
  viewer.setSize (800, 600);

  // ############################################################################
  // load point cloud
  printf ("  loading %s\n", pcd_file.c_str ());
  pcl::PointCloud<Point>::Ptr cloud (new pcl::PointCloud<Point>);
  pcl::PCLPointCloud2 cloud2;
  pcl::on_nurbs::NurbsDataSurface data;

  if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
    throw std::runtime_error ("  PCD file not found.");

  fromPCLPointCloud2 (cloud2, *cloud);
  PointCloud2Vector3d (cloud, data.interior);
  pcl::visualization::PointCloudColorHandlerCustom<Point> handler (cloud, 0, 255, 0);
  viewer.addPointCloud<Point> (cloud, handler, "cloud_cylinder");
  printf ("  %zu points in data set\n", cloud->size ());

  // ############################################################################
  // fit NURBS surface
  printf ("  surface fitting ...\n");
  ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (order, &data);
  pcl::on_nurbs::FittingSurface fit (&data, nurbs);
  //  fit.setQuiet (false);

  pcl::PolygonMesh mesh;
  pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<pcl::Vertices> mesh_vertices;

  std::string mesh_id = "mesh_nurbs";
  pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (fit.m_nurbs, mesh, mesh_resolution);
  viewer.addPolygonMesh (mesh, mesh_id);

  pcl::on_nurbs::FittingSurface::Parameter params;
  params.interior_smoothness = 0.15;
  params.interior_weight = 1.0;
  params.boundary_smoothness = 0.15;
  params.boundary_weight = 0.0;

  // NURBS refinement
  for (unsigned i = 0; i < refinement; i++)
  {
    fit.refine (0);
    fit.refine (1);
    fit.assemble (params);
    fit.solve ();
    pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
    viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
    viewer.spinOnce ();
  }

  // fitting iterations
  for (unsigned i = 0; i < iterations; i++)
  {
    fit.assemble (params);
    fit.solve ();
    pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
    viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
    viewer.spinOnce ();
  }

  // ############################################################################
  // fit NURBS curve
  pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
  curve_params.addCPsAccuracy = 3e-2;
  curve_params.addCPsIteration = 3;
  curve_params.maxCPs = 200;
  curve_params.accuracy = 1e-3;
  curve_params.iterations = 10000;

  curve_params.param.closest_point_resolution = 0;
  curve_params.param.closest_point_weight = 1.0;
  curve_params.param.closest_point_sigma2 = 0.1;
  curve_params.param.interior_sigma2 = 0.0001;
  curve_params.param.smooth_concavity = 1.0;
  curve_params.param.smoothness = 1.0;

  pcl::on_nurbs::NurbsDataCurve2d curve_data;
  curve_data.interior = data.interior_param;
  curve_data.interior_weight_function.push_back (true);

  ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D (order, curve_data.interior);

  pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
//  curve_fit.setQuiet (false);

  // ############################### FITTING ###############################
  curve_fit.fitting (curve_params);
  visualizeCurve (curve_fit.m_nurbs, fit.m_nurbs, viewer);

  // ############################################################################
  // triangulation of trimmed surface
  printf ("  triangulate trimmed surface ...\n");
  viewer.removePolygonMesh (mesh_id);
  pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (fit.m_nurbs, curve_fit.m_nurbs, mesh,
                                                                   mesh_resolution);
  viewer.addPolygonMesh (mesh, mesh_id);

  printf ("  ... done.\n");

  viewer.spin ();
  return 0;
}
<commit_msg>NURBS fitting: fixed trimming curve for surface fitting (parameter was wrong)<commit_after>#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/surface/on_nurbs/fitting_surface_tdm.h>
#include <pcl/surface/on_nurbs/fitting_curve_2d_asdm.h>
#include <pcl/surface/on_nurbs/triangulation.h>

typedef pcl::PointXYZ Point;

void
PointCloud2Vector3d (pcl::PointCloud<Point>::Ptr cloud, pcl::on_nurbs::vector_vec3d &data)
{
  for (unsigned i = 0; i < cloud->size (); i++)
  {
    Point &p = cloud->at (i);
    if (!pcl_isnan (p.x) && !pcl_isnan (p.y) && !pcl_isnan (p.z))
      data.push_back (Eigen::Vector3d (p.x, p.y, p.z));
  }
}

void
visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer)
{
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

  pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4);
  for (std::size_t i = 0; i < curve_cloud->size () - 1; i++)
  {
    pcl::PointXYZRGB &p1 = curve_cloud->at (i);
    pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1);
    std::ostringstream os;
    os << "line" << i;
    viewer.removeShape (os.str ());
    viewer.addLine<pcl::PointXYZRGB> (p1, p2, 1.0, 0.0, 0.0, os.str ());
  }

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps (new pcl::PointCloud<pcl::PointXYZRGB>);
  for (int i = 0; i < curve.CVCount (); i++)
  {
    ON_3dPoint p1;
    curve.GetCV (i, p1);

    double pnt[3];
    surface.Evaluate (p1.x, p1.y, 0, 3, pnt);
    pcl::PointXYZRGB p2;
    p2.x = float (pnt[0]);
    p2.y = float (pnt[1]);
    p2.z = float (pnt[2]);

    p2.r = 255;
    p2.g = 0;
    p2.b = 0;

    curve_cps->push_back (p2);
  }
  viewer.removePointCloud ("cloud_cps");
  viewer.addPointCloud (curve_cps, "cloud_cps");
}

int
main (int argc, char *argv[])
{
  std::string pcd_file;

  if (argc < 2)
  {
    printf ("\nUsage: pcl_example_nurbs_fitting_surface pcd<PointXYZ>-file\n\n");
    exit (0);
  }

  pcd_file = argv[1];

  unsigned order (3);
  unsigned refinement (5);
  unsigned iterations (10);
  unsigned mesh_resolution (256);

  pcl::visualization::PCLVisualizer viewer ("Test: NURBS surface fitting");
  viewer.setSize (800, 600);

  // ############################################################################
  // load point cloud
  printf ("  loading %s\n", pcd_file.c_str ());
  pcl::PointCloud<Point>::Ptr cloud (new pcl::PointCloud<Point>);
  pcl::PCLPointCloud2 cloud2;
  pcl::on_nurbs::NurbsDataSurface data;

  if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
    throw std::runtime_error ("  PCD file not found.");

  fromPCLPointCloud2 (cloud2, *cloud);
  PointCloud2Vector3d (cloud, data.interior);
  pcl::visualization::PointCloudColorHandlerCustom<Point> handler (cloud, 0, 255, 0);
  viewer.addPointCloud<Point> (cloud, handler, "cloud_cylinder");
  printf ("  %zu points in data set\n", cloud->size ());

  // ############################################################################
  // fit NURBS surface
  printf ("  surface fitting ...\n");
  ON_NurbsSurface nurbs = pcl::on_nurbs::FittingSurface::initNurbsPCABoundingBox (order, &data);
  pcl::on_nurbs::FittingSurface fit (&data, nurbs);
  //  fit.setQuiet (false);

  pcl::PolygonMesh mesh;
  pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  std::vector<pcl::Vertices> mesh_vertices;

  std::string mesh_id = "mesh_nurbs";
  pcl::on_nurbs::Triangulation::convertSurface2PolygonMesh (fit.m_nurbs, mesh, mesh_resolution);
  viewer.addPolygonMesh (mesh, mesh_id);

  pcl::on_nurbs::FittingSurface::Parameter params;
  params.interior_smoothness = 0.15;
  params.interior_weight = 1.0;
  params.boundary_smoothness = 0.15;
  params.boundary_weight = 0.0;

  // NURBS refinement
  for (unsigned i = 0; i < refinement; i++)
  {
    fit.refine (0);
    fit.refine (1);
    fit.assemble (params);
    fit.solve ();
    pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
    viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
    viewer.spinOnce ();
  }

  // fitting iterations
  for (unsigned i = 0; i < iterations; i++)
  {
    fit.assemble (params);
    fit.solve ();
    pcl::on_nurbs::Triangulation::convertSurface2Vertices (fit.m_nurbs, mesh_cloud, mesh_vertices, mesh_resolution);
    viewer.updatePolygonMesh<pcl::PointXYZ> (mesh_cloud, mesh_vertices, mesh_id);
    viewer.spinOnce ();
  }

  // ############################################################################
  // fit NURBS curve
  pcl::on_nurbs::FittingCurve2dAPDM::FitParameter curve_params;
  curve_params.addCPsAccuracy = 5e-2;
  curve_params.addCPsIteration = 3;
  curve_params.maxCPs = 200;
  curve_params.accuracy = 1e-3;
  curve_params.iterations = 100;

  curve_params.param.closest_point_resolution = 0;
  curve_params.param.closest_point_weight = 1.0;
  curve_params.param.closest_point_sigma2 = 0.1;
  curve_params.param.interior_sigma2 = 0.0001;
  curve_params.param.smooth_concavity = 1.0;
  curve_params.param.smoothness = 1.0;

  pcl::on_nurbs::NurbsDataCurve2d curve_data;
  curve_data.interior = data.interior_param;
  curve_data.interior_weight_function.push_back (true);

  ON_NurbsCurve curve_nurbs = pcl::on_nurbs::FittingCurve2dAPDM::initNurbsCurve2D (order, curve_data.interior);

  pcl::on_nurbs::FittingCurve2dASDM curve_fit (&curve_data, curve_nurbs);
//  curve_fit.setQuiet (false);

  // ############################### FITTING ###############################
  curve_fit.fitting (curve_params);
  visualizeCurve (curve_fit.m_nurbs, fit.m_nurbs, viewer);

  // ############################################################################
  // triangulation of trimmed surface
  printf ("  triangulate trimmed surface ...\n");
  viewer.removePolygonMesh (mesh_id);
  pcl::on_nurbs::Triangulation::convertTrimmedSurface2PolygonMesh (fit.m_nurbs, curve_fit.m_nurbs, mesh,
                                                                   mesh_resolution);
  viewer.addPolygonMesh (mesh, mesh_id);

  printf ("  ... done.\n");

  viewer.spin ();
  return 0;
}
<|endoftext|>


<commit_before>/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *                    _______    _                                     *
 *                   (  ____ \  ( \     |\     /|                      *
 *                   | (    \/  | (     ( \   / )                      *
 *                   | (__      | |      \ (_) /                       *
 *                   |  __)     | |       \   /                        *
 *                   | (        | |        ) (                         *
 *                   | )        | (____/\  | |                         *
 *                   |/         (_______/  \_/                         *
 *                                                                     *
 *                                                                     *
 *     fly is an awesome c++11 network library.                        *
 *                                                                     *
 *   @author: lichuan                                                  *
 *   @qq: 308831759                                                    *
 *   @email: 308831759@qq.com                                          *
 *   @github: https://github.com/lichuan/fly                           *
 *   @date: 2015-06-24 20:43:56                                        *
 *                                                                     *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <sys/epoll.h>
#include <sys/eventfd.h>
#include <unistd.h>
#include "fly/base/logger.hpp"
#include "fly/net/poller_task.hpp"

namespace fly {
namespace net {

template<typename T>
Poller_Task<T>::Poller_Task(uint64 seq) : Loop_Task(seq)
{
    m_fd = epoll_create1(0);

    if(m_fd < 0)
    {
        LOG_FATAL("epoll_create1 failed in Poller_Task::Poller_Task %s", strerror(errno));
        return;
    }

    m_close_event_fd = eventfd(0, 0);

    if(m_close_event_fd < 0)
    {
        LOG_FATAL("close event eventfd failed in Poller_Task::Poller_Task");
        return;
    }

    m_write_event_fd = eventfd(0, 0);

    if(m_write_event_fd < 0)
    {
        LOG_FATAL("write event eventfd failed in Poller_Task::Poller_Task");
        return;
    }

    m_stop_event_fd = eventfd(0, 0);

    if(m_stop_event_fd < 0)
    {
        LOG_FATAL("stop event eventfd failed in Poller_Task::Poller_Task");
        return;
    }

    struct epoll_event event;
    m_close_udata.reset(new Connection<T>(m_close_event_fd, Addr("close_event", 0)));
    event.data.ptr = m_close_udata.get();
    event.events = EPOLLIN;
    int32 ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, m_close_event_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("close event epoll_ctl failed in Poller_Task::Poller_Task");
        return;
    }

    m_write_udata.reset(new Connection<T>(m_write_event_fd, Addr("write_event", 0)));
    event.data.ptr = m_write_udata.get();
    ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, m_write_event_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("write event epoll_ctl failed in Poller_Task::Poller_Task");
        return;
    }

    m_stop_udata.reset(new Connection<T>(m_stop_event_fd, Addr("stop_event", 0)));
    event.data.ptr = m_stop_udata.get();
    ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, m_stop_event_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("stop event epoll_ctl failed in Poller_Task::Poller_Task");
    }
}

template<typename T>
bool Poller_Task<T>::register_connection(std::shared_ptr<Connection<T>> connection)
{
    struct epoll_event event;
    event.data.ptr = connection.get();
    event.events = EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLET;
    connection->m_poller_task = this;
    connection->m_self = connection;
    connection->m_init_cb(connection);
    int32 ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, connection->m_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("epoll_ctl failed in Poller_Task::register_connection: %s", strerror(errno));
        close(connection->m_fd);
        connection->m_closed.store(true, std::memory_order_relaxed);
        connection->m_be_closed_cb(connection->shared_from_this());
        connection->m_self.reset();

        return false;
    }

    return true;
}

template<typename T>
void Poller_Task<T>::close_connection(std::shared_ptr<Connection<T>> connection)
{
    m_close_queue.push(connection);
    uint64 data = 1;
    int32 num = write(m_close_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("write m_close_event_fd failed in Poller_Task::close_connection");
    }
}

template<typename T>
void Poller_Task<T>::stop()
{
    uint64 data = 1;
    int32 num = write(m_stop_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("write m_stop_event_fd failed in Poller_Task::stop");
    }
}

template<typename T>
void Poller_Task<T>::write_connection(std::shared_ptr<Connection<T>> connection)
{
    m_write_queue.push(connection);
    uint64 data = 1;
    int32 num = write(m_write_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("write m_write_event_fd failed in Poller_Task::write_connection");
    }
}

template<typename T>
void Poller_Task<T>::do_write(std::shared_ptr<Connection<T>> connection)
{
    int32 fd = connection->m_fd;
    Message_Chunk_Queue &send_queue = connection->m_send_msg_queue;

    while(Message_Chunk *message_chunk = send_queue.pop())
    {
        int32 message_length = message_chunk->length();
        int32 num = write(fd, message_chunk->read_ptr(), message_length);

        if(num < 0)
        {
            if(errno == EAGAIN || errno == EWOULDBLOCK)
            {
                send_queue.push_front(message_chunk);

                break;
            }
        }

        if(num == 0)
        {
            LOG_FATAL("write return 0, it's impossible, in Poller_Task::do_write(arg)");
        }

        if(num <= 0)
        {
            epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);
            close(fd);
            connection->m_self.reset();
            connection->m_closed.store(true, std::memory_order_relaxed);
            connection->m_be_closed_cb(connection);

            break;
        }

        if(num < message_length)
        {
            message_chunk->read_ptr(num);
            send_queue.push_front(message_chunk);

            break;
        }
        else
        {
            delete message_chunk;
        }
    }
}

template<typename T>
void Poller_Task<T>::do_write()
{
    uint64 data = 0;
    int32 num = read(m_write_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("read m_write_event_fd failed in Poller_Task::do_write");

        return;
    }

    std::list<std::shared_ptr<Connection<T>>> write_queue;

    if(m_write_queue.pop(write_queue))
    {
        for(auto &connection : write_queue)
        {
            if(!connection->m_closed.load(std::memory_order_relaxed))
            {
                do_write(connection);
            }
        }
    }
}

template<typename T>
void Poller_Task<T>::do_close()
{
    uint64 data = 0;
    int32 num = read(m_close_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("read m_close_event_fd failed in Poller_Task::do_close");

        return;
    }

    std::list<std::shared_ptr<Connection<T>>> close_queue;

    if(m_close_queue.pop(close_queue))
    {
        for(auto &connection : close_queue)
        {
            if(!connection->m_closed.load(std::memory_order_relaxed))
            {
                int32 fd = connection->m_fd;
                int ret = epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);

                if(ret < 0)
                {
                    LOG_FATAL("epoll_ctl EPOLL_CTL_DEL failed in Poller_Task::do_close: %s", strerror(errno));
                }

                close(fd);
                connection->m_self.reset();
                connection->m_closed.store(true, std::memory_order_relaxed);
                connection->m_close_cb(connection);
            }
        }
    }
}

template<typename T>
void Poller_Task<T>::run_in_loop()
{
    struct epoll_event events[2048];
    int32 fd_num = epoll_wait(m_fd, events, 2048, -1);

    if(fd_num < 0)
    {
        LOG_FATAL("epoll_wait failed in Poller_Task::run_in_loop %s", strerror(errno));

        return;
    }

    for(auto i = 0; i < fd_num; ++i)
    {
        Connection<T> *connection = static_cast<Connection<T>*>(events[i].data.ptr);
        int32 fd = connection->m_fd;
        uint32 event = events[i].events;

        if(event & (EPOLLERR | EPOLLHUP | EPOLLRDHUP))
        {
            if(connection->m_closed.load(std::memory_order_relaxed))
            {
                continue;
            }

            int ret = epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);

            if(ret < 0)
            {
                LOG_FATAL("epoll_ctl EPOLL_CTL_DEL failed in Poller_Task::run_in_loop: %s", strerror(errno));
            }

            close(fd);
            connection->m_closed.store(true, std::memory_order_relaxed);
            connection->m_be_closed_cb(connection->shared_from_this());
            connection->m_self.reset();
        }
        else if(event & EPOLLIN)
        {
            if(fd == m_close_event_fd)
            {
                do_close();
            }
            else if(fd == m_write_event_fd)
            {
                do_write();
            }
            else if(fd == m_stop_event_fd)
            {
                Loop_Task::stop();
                close(m_fd);
                break;
            }
            else
            {
                if(connection->m_closed.load(std::memory_order_relaxed))
                {
                    continue;
                }

                while(true)
                {
                    const uint32 REQ_SIZE = 100 * 1024;
                    Message_Chunk_Queue &recv_queue = connection->m_recv_msg_queue;
                    std::unique_ptr<Message_Chunk> message_chunk(new Message_Chunk(REQ_SIZE));
                    int32 num = read(fd, message_chunk->read_ptr(), REQ_SIZE);

                    if(num < 0)
                    {
                        if(errno == EAGAIN || errno == EWOULDBLOCK)
                        {
                            break;
                        }
                    }

                    if(num <= 0)
                    {
                        epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);
                        close(fd);
                        connection->m_closed.store(true, std::memory_order_relaxed);
                        connection->m_be_closed_cb(connection->shared_from_this());
                        connection->m_self.reset();

                        break;
                    }

                    message_chunk->write_ptr(num);
                    recv_queue.push(message_chunk.release());
                    connection->parse();

                    if(num < REQ_SIZE)
                    {
                        break;
                    }
                }
            }
        }

        if(event & EPOLLOUT)
        {
            if(connection->m_closed.load(std::memory_order_relaxed))
            {
                continue;
            }

            do_write(connection->shared_from_this());
        }
    }
}

template class Poller_Task<Json>;
template class Poller_Task<Wsock>;
template class Poller_Task<Proto>;

}
}
<commit_msg>change req size to 2M<commit_after>/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
 *                    _______    _                                     *
 *                   (  ____ \  ( \     |\     /|                      *
 *                   | (    \/  | (     ( \   / )                      *
 *                   | (__      | |      \ (_) /                       *
 *                   |  __)     | |       \   /                        *
 *                   | (        | |        ) (                         *
 *                   | )        | (____/\  | |                         *
 *                   |/         (_______/  \_/                         *
 *                                                                     *
 *                                                                     *
 *     fly is an awesome c++11 network library.                        *
 *                                                                     *
 *   @author: lichuan                                                  *
 *   @qq: 308831759                                                    *
 *   @email: 308831759@qq.com                                          *
 *   @github: https://github.com/lichuan/fly                           *
 *   @date: 2015-06-24 20:43:56                                        *
 *                                                                     *
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#include <sys/epoll.h>
#include <sys/eventfd.h>
#include <unistd.h>
#include "fly/base/logger.hpp"
#include "fly/net/poller_task.hpp"

namespace fly {
namespace net {

template<typename T>
Poller_Task<T>::Poller_Task(uint64 seq) : Loop_Task(seq)
{
    m_fd = epoll_create1(0);

    if(m_fd < 0)
    {
        LOG_FATAL("epoll_create1 failed in Poller_Task::Poller_Task %s", strerror(errno));
        return;
    }

    m_close_event_fd = eventfd(0, 0);

    if(m_close_event_fd < 0)
    {
        LOG_FATAL("close event eventfd failed in Poller_Task::Poller_Task");
        return;
    }

    m_write_event_fd = eventfd(0, 0);

    if(m_write_event_fd < 0)
    {
        LOG_FATAL("write event eventfd failed in Poller_Task::Poller_Task");
        return;
    }

    m_stop_event_fd = eventfd(0, 0);

    if(m_stop_event_fd < 0)
    {
        LOG_FATAL("stop event eventfd failed in Poller_Task::Poller_Task");
        return;
    }

    struct epoll_event event;
    m_close_udata.reset(new Connection<T>(m_close_event_fd, Addr("close_event", 0)));
    event.data.ptr = m_close_udata.get();
    event.events = EPOLLIN;
    int32 ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, m_close_event_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("close event epoll_ctl failed in Poller_Task::Poller_Task");
        return;
    }

    m_write_udata.reset(new Connection<T>(m_write_event_fd, Addr("write_event", 0)));
    event.data.ptr = m_write_udata.get();
    ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, m_write_event_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("write event epoll_ctl failed in Poller_Task::Poller_Task");
        return;
    }

    m_stop_udata.reset(new Connection<T>(m_stop_event_fd, Addr("stop_event", 0)));
    event.data.ptr = m_stop_udata.get();
    ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, m_stop_event_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("stop event epoll_ctl failed in Poller_Task::Poller_Task");
    }
}

template<typename T>
bool Poller_Task<T>::register_connection(std::shared_ptr<Connection<T>> connection)
{
    struct epoll_event event;
    event.data.ptr = connection.get();
    event.events = EPOLLIN | EPOLLOUT | EPOLLRDHUP | EPOLLET;
    connection->m_poller_task = this;
    connection->m_self = connection;
    connection->m_init_cb(connection);
    int32 ret = epoll_ctl(m_fd, EPOLL_CTL_ADD, connection->m_fd, &event);

    if(ret < 0)
    {
        LOG_FATAL("epoll_ctl failed in Poller_Task::register_connection: %s", strerror(errno));
        close(connection->m_fd);
        connection->m_closed.store(true, std::memory_order_relaxed);
        connection->m_be_closed_cb(connection->shared_from_this());
        connection->m_self.reset();

        return false;
    }

    return true;
}

template<typename T>
void Poller_Task<T>::close_connection(std::shared_ptr<Connection<T>> connection)
{
    m_close_queue.push(connection);
    uint64 data = 1;
    int32 num = write(m_close_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("write m_close_event_fd failed in Poller_Task::close_connection");
    }
}

template<typename T>
void Poller_Task<T>::stop()
{
    uint64 data = 1;
    int32 num = write(m_stop_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("write m_stop_event_fd failed in Poller_Task::stop");
    }
}

template<typename T>
void Poller_Task<T>::write_connection(std::shared_ptr<Connection<T>> connection)
{
    m_write_queue.push(connection);
    uint64 data = 1;
    int32 num = write(m_write_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("write m_write_event_fd failed in Poller_Task::write_connection");
    }
}

template<typename T>
void Poller_Task<T>::do_write(std::shared_ptr<Connection<T>> connection)
{
    int32 fd = connection->m_fd;
    Message_Chunk_Queue &send_queue = connection->m_send_msg_queue;

    while(Message_Chunk *message_chunk = send_queue.pop())
    {
        int32 message_length = message_chunk->length();
        int32 num = write(fd, message_chunk->read_ptr(), message_length);

        if(num < 0)
        {
            if(errno == EAGAIN || errno == EWOULDBLOCK)
            {
                send_queue.push_front(message_chunk);

                break;
            }
        }

        if(num == 0)
        {
            LOG_FATAL("write return 0, it's impossible, in Poller_Task::do_write(arg)");
        }

        if(num <= 0)
        {
            epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);
            close(fd);
            connection->m_self.reset();
            connection->m_closed.store(true, std::memory_order_relaxed);
            connection->m_be_closed_cb(connection);

            break;
        }

        if(num < message_length)
        {
            message_chunk->read_ptr(num);
            send_queue.push_front(message_chunk);

            break;
        }
        else
        {
            delete message_chunk;
        }
    }
}

template<typename T>
void Poller_Task<T>::do_write()
{
    uint64 data = 0;
    int32 num = read(m_write_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("read m_write_event_fd failed in Poller_Task::do_write");

        return;
    }

    std::list<std::shared_ptr<Connection<T>>> write_queue;

    if(m_write_queue.pop(write_queue))
    {
        for(auto &connection : write_queue)
        {
            if(!connection->m_closed.load(std::memory_order_relaxed))
            {
                do_write(connection);
            }
        }
    }
}

template<typename T>
void Poller_Task<T>::do_close()
{
    uint64 data = 0;
    int32 num = read(m_close_event_fd, &data, sizeof(uint64));

    if(num != sizeof(uint64))
    {
        LOG_FATAL("read m_close_event_fd failed in Poller_Task::do_close");

        return;
    }

    std::list<std::shared_ptr<Connection<T>>> close_queue;

    if(m_close_queue.pop(close_queue))
    {
        for(auto &connection : close_queue)
        {
            if(!connection->m_closed.load(std::memory_order_relaxed))
            {
                int32 fd = connection->m_fd;
                int ret = epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);

                if(ret < 0)
                {
                    LOG_FATAL("epoll_ctl EPOLL_CTL_DEL failed in Poller_Task::do_close: %s", strerror(errno));
                }

                close(fd);
                connection->m_self.reset();
                connection->m_closed.store(true, std::memory_order_relaxed);
                connection->m_close_cb(connection);
            }
        }
    }
}

template<typename T>
void Poller_Task<T>::run_in_loop()
{
    struct epoll_event events[2048];
    int32 fd_num = epoll_wait(m_fd, events, 2048, -1);

    if(fd_num < 0)
    {
        LOG_FATAL("epoll_wait failed in Poller_Task::run_in_loop %s", strerror(errno));

        return;
    }

    for(auto i = 0; i < fd_num; ++i)
    {
        Connection<T> *connection = static_cast<Connection<T>*>(events[i].data.ptr);
        int32 fd = connection->m_fd;
        uint32 event = events[i].events;

        if(event & (EPOLLERR | EPOLLHUP | EPOLLRDHUP))
        {
            if(connection->m_closed.load(std::memory_order_relaxed))
            {
                continue;
            }

            int ret = epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);

            if(ret < 0)
            {
                LOG_FATAL("epoll_ctl EPOLL_CTL_DEL failed in Poller_Task::run_in_loop: %s", strerror(errno));
            }

            close(fd);
            connection->m_closed.store(true, std::memory_order_relaxed);
            connection->m_be_closed_cb(connection->shared_from_this());
            connection->m_self.reset();
        }
        else if(event & EPOLLIN)
        {
            if(fd == m_close_event_fd)
            {
                do_close();
            }
            else if(fd == m_write_event_fd)
            {
                do_write();
            }
            else if(fd == m_stop_event_fd)
            {
                Loop_Task::stop();
                close(m_fd);
                break;
            }
            else
            {
                if(connection->m_closed.load(std::memory_order_relaxed))
                {
                    continue;
                }

                while(true)
                {
                    const uint32 REQ_SIZE = 2 * 1024 * 1024;
                    Message_Chunk_Queue &recv_queue = connection->m_recv_msg_queue;
                    std::unique_ptr<Message_Chunk> message_chunk(new Message_Chunk(REQ_SIZE));
                    int32 num = read(fd, message_chunk->read_ptr(), REQ_SIZE);

                    if(num < 0)
                    {
                        if(errno == EAGAIN || errno == EWOULDBLOCK)
                        {
                            break;
                        }
                    }

                    if(num <= 0)
                    {
                        epoll_ctl(m_fd, EPOLL_CTL_DEL, fd, NULL);
                        close(fd);
                        connection->m_closed.store(true, std::memory_order_relaxed);
                        connection->m_be_closed_cb(connection->shared_from_this());
                        connection->m_self.reset();

                        break;
                    }

                    message_chunk->write_ptr(num);
                    recv_queue.push(message_chunk.release());
                    connection->parse();

                    if(num < REQ_SIZE)
                    {
                        break;
                    }
                }
            }
        }

        if(event & EPOLLOUT)
        {
            if(connection->m_closed.load(std::memory_order_relaxed))
            {
                continue;
            }

            do_write(connection->shared_from_this());
        }
    }
}

template class Poller_Task<Json>;
template class Poller_Task<Wsock>;
template class Poller_Task<Proto>;

}
}
<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
/// @brief batch request handler
///
/// @file
///
/// DISCLAIMER
///
/// Copyright 2004-2012 triagens GmbH, Cologne, Germany
///
/// 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.
///
/// Copyright holder is triAGENS GmbH, Cologne, Germany
///
/// @author Jan Steemann
/// @author Copyright 2012, triAGENS GmbH, Cologne, Germany
////////////////////////////////////////////////////////////////////////////////

#include "RestBatchHandler.h"

#include "Basics/StringUtils.h"
#include "Basics/MutexLocker.h"
#include "Rest/HttpRequest.h"
#include "Rest/HttpResponse.h"
#include "VocBase/vocbase.h"
#include "GeneralServer/GeneralCommTask.h"
#include "GeneralServer/GeneralServerJob.h"
#include "HttpServer/HttpHandler.h"
#include "HttpServer/HttpServer.h"
#include "ProtocolBuffers/HttpRequestProtobuf.h"

using namespace std;
using namespace triagens::basics;
using namespace triagens::rest;
using namespace triagens::arango;

// -----------------------------------------------------------------------------
// --SECTION--                                      constructors and destructors
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @addtogroup ArangoDB
/// @{
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/// @brief constructor
////////////////////////////////////////////////////////////////////////////////

RestBatchHandler::RestBatchHandler (HttpRequest* request, TRI_vocbase_t* vocbase)
  : RestVocbaseBaseHandler(request, vocbase),
    _missingResponses(0),
    _reallyDone(0),
    _outputMessages(new PB_ArangoMessage) {
}

////////////////////////////////////////////////////////////////////////////////
/// @brief destructor
////////////////////////////////////////////////////////////////////////////////

RestBatchHandler::~RestBatchHandler () {
  // delete protobuf message
  delete _outputMessages;

  // clear all handlers that still exist
  for (size_t i = 0; i < _handlers.size(); ++i) {
    HttpHandler* handler = _handlers[i];

    if (handler != 0) {
      _task->getServer()->destroyHandler(handler);
      _handlers[i] = 0;
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @}
////////////////////////////////////////////////////////////////////////////////

// -----------------------------------------------------------------------------
// --SECTION--                                                   Handler methods
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @addtogroup ArangoDB
/// @{
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

bool RestBatchHandler::isDirect () {
  return false;
}

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

string const& RestBatchHandler::queue () {
  static string const client = "STANDARD";

  return client;
}

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

HttpHandler::status_e RestBatchHandler::execute () {
  // extract the request type
  HttpRequest::HttpRequestType type = request->requestType();
  string contentType = StringUtils::tolower(StringUtils::trim(request->header("content-type")));

  if (type != HttpRequest::HTTP_REQUEST_POST || contentType != getContentType()) {
    generateNotImplemented("ILLEGAL " + BATCH_PATH);
    return HANDLER_DONE;
  }

  /*
  FILE* fp = fopen("got","w");
  fwrite(request->body(), request->bodySize(), 1, fp);
  fclose(fp);
  */
  PB_ArangoMessage inputMessages;

  bool result = inputMessages.ParseFromArray(request->body(), request->bodySize());
  if (!result) {
    generateError(HttpResponse::BAD,
                  TRI_ERROR_ARANGO_COLLECTION_PARAMETER_MISSING, // TODO FIXME
                  "invalid protobuf message");
    return HANDLER_DONE;
  }

  bool failed = false;
  bool hasAsync = false;

  // loop over the input messages once to set up the output structures without concurrency
  for (int i = 0; i < inputMessages.messages_size(); ++i) {
    _outputMessages->add_messages();

    // create a handler for each input part
    const PB_ArangoBatchMessage inputMessage = inputMessages.messages(i);
    HttpRequestProtobuf* request = new HttpRequestProtobuf(inputMessage);
    HttpHandler* handler = _task->getServer()->createHandler(request);

    if (!handler) {
      failed = true;
      break;
    }
    else {
      _handlers.push_back(handler);
      if (!handler->isDirect()) {
        // async handler
        ++_missingResponses;
        hasAsync = true;
      }
    }
  }

  if (failed) {
    // TODO: handle error!
    std::cout << "SOMETHING FAILED--------------------------------------------------------------------------\n";
    return Handler::HANDLER_DONE;
  }

  try {
  // now loop again with all output structures set up
  for (int i = 0; i < inputMessages.messages_size(); ++i) {
    const PB_ArangoBatchMessage inputMessage = inputMessages.messages(i);

    HttpHandler* handler = _handlers[i];

    assert(handler);

    if (handler->isDirect()) {
      // execute handler directly
      Handler::status_e status = Handler::HANDLER_FAILED;

      try {
        status = handler->execute();
      }
      catch (...) {
        // TODO
      }

      if (status != Handler::HANDLER_REQUEUE) {
        addResponse(handler);
      }
    }
    else {
      // execute handler via dispatcher
      HttpServer* server = dynamic_cast<HttpServer*>(_task->getServer());

      Scheduler* scheduler = server->getScheduler();
      Dispatcher* dispatcher = server->getDispatcher();

      Job* job = handler->createJob(scheduler, dispatcher, _task);

      GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler>* generalJob =
        dynamic_cast<GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler> * >(job);
      generalJob->attachObserver(this);

      dispatcher->addJob(job);
    }
  }
  }
  catch (...) {
    std::cout << "SOMETHING WENT WRONG - EXCEPTION\n";
  }

  if (!hasAsync) {
    _reallyDone = 1;
    return Handler::HANDLER_DONE;
  }

  // we have async jobs
  return Handler::HANDLER_DETACH;
}

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

bool RestBatchHandler::handleAsync () {
  if (_reallyDone) {
    assembleResponse();
    toServerJob(_job)->setDone();
    return HttpHandler::handleAsync();
  }

  return true;
}

////////////////////////////////////////////////////////////////////////////////
/// notification routine called by async sub jobs
////////////////////////////////////////////////////////////////////////////////

void RestBatchHandler::notify (Job* job, const Job::notification_e type) {
  if (type != Job::JOB_CLEANUP) {
    return;
  }

  assert(_reallyDone == 0);
  HttpHandler* handler = toServerJob(job)->getHandler();
  addResponse(handler);

  if (--_missingResponses == 0) {
    _reallyDone = 1;

    // signal to the task that we are done
    GeneralAsyncCommTask<HttpServer, HttpHandlerFactory, HttpCommTask>* atask =
      dynamic_cast<GeneralAsyncCommTask<HttpServer, HttpHandlerFactory, HttpCommTask>*>(_task);
    atask->signal();
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief add a single handler response to the output array
////////////////////////////////////////////////////////////////////////////////

void RestBatchHandler::addResponse (HttpHandler* handler) {
  for (size_t i = 0; i < _handlers.size(); ++i) {
    if (_handlers[i] == handler) {
      // avoid concurrent modifications to the structure
      MUTEX_LOCKER(_handlerLock);
      PB_ArangoBatchMessage* batch = _outputMessages->mutable_messages(i);

      handler->getResponse()->write(batch);
      // delete the handler
      _task->getServer()->destroyHandler(handler);
      _handlers[i] = 0;
      return;
    }
  }

  // handler not found
  LOGGER_WARNING << "handler not found. this should not happen.";
}

////////////////////////////////////////////////////////////////////////////////
/// @brief create an overall protobuf response from the array of responses
////////////////////////////////////////////////////////////////////////////////

void RestBatchHandler::assembleResponse () {
  assert(_missingResponses == 0);

  response = new HttpResponse(HttpResponse::OK);
  response->setContentType(getContentType());

  string data;
  if (!_outputMessages->SerializeToString(&data)) {
    // TODO
  }
  response->body().appendText(data);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief convert a Job* to a GeneralServerJob*
////////////////////////////////////////////////////////////////////////////////

GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler>* RestBatchHandler::toServerJob(Job* job) {
  return dynamic_cast<GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler> * >(job);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief return the required content type string
////////////////////////////////////////////////////////////////////////////////

string const& RestBatchHandler::getContentType () {
  static string const contentType = "application/x-protobuf";

  return contentType;
}

////////////////////////////////////////////////////////////////////////////////
/// @}
////////////////////////////////////////////////////////////////////////////////

// Local Variables:
// mode: outline-minor
// outline-regexp: "^\\(/// @brief\\|/// {@inheritDoc}\\|/// @addtogroup\\|// --SECTION--\\|/// @\\}\\)"
// End:
<commit_msg>some cleanup<commit_after>////////////////////////////////////////////////////////////////////////////////
/// @brief batch request handler
///
/// @file
///
/// DISCLAIMER
///
/// Copyright 2004-2012 triagens GmbH, Cologne, Germany
///
/// 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.
///
/// Copyright holder is triAGENS GmbH, Cologne, Germany
///
/// @author Jan Steemann
/// @author Copyright 2012, triAGENS GmbH, Cologne, Germany
////////////////////////////////////////////////////////////////////////////////

#include "RestBatchHandler.h"

#include "Basics/StringUtils.h"
#include "Basics/MutexLocker.h"
#include "Rest/HttpRequest.h"
#include "Rest/HttpResponse.h"
#include "VocBase/vocbase.h"
#include "GeneralServer/GeneralCommTask.h"
#include "GeneralServer/GeneralServerJob.h"
#include "HttpServer/HttpHandler.h"
#include "HttpServer/HttpServer.h"
#include "ProtocolBuffers/HttpRequestProtobuf.h"

using namespace std;
using namespace triagens::basics;
using namespace triagens::rest;
using namespace triagens::arango;

// -----------------------------------------------------------------------------
// --SECTION--                                      constructors and destructors
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @addtogroup ArangoDB
/// @{
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/// @brief constructor
////////////////////////////////////////////////////////////////////////////////

RestBatchHandler::RestBatchHandler (HttpRequest* request, TRI_vocbase_t* vocbase)
  : RestVocbaseBaseHandler(request, vocbase),
    _missingResponses(0),
    _reallyDone(0),
    _outputMessages(new PB_ArangoMessage) {
}

////////////////////////////////////////////////////////////////////////////////
/// @brief destructor
////////////////////////////////////////////////////////////////////////////////

RestBatchHandler::~RestBatchHandler () {
  // delete protobuf message
  delete _outputMessages;

  // clear all handlers that still exist
  for (size_t i = 0; i < _handlers.size(); ++i) {
    HttpHandler* handler = _handlers[i];

    if (handler != 0) {
      _task->getServer()->destroyHandler(handler);
      _handlers[i] = 0;
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @}
////////////////////////////////////////////////////////////////////////////////

// -----------------------------------------------------------------------------
// --SECTION--                                                   Handler methods
// -----------------------------------------------------------------------------

////////////////////////////////////////////////////////////////////////////////
/// @addtogroup ArangoDB
/// @{
////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

bool RestBatchHandler::isDirect () {
  return false;
}

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

string const& RestBatchHandler::queue () {
  static string const client = "STANDARD";

  return client;
}

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

HttpHandler::status_e RestBatchHandler::execute () {
  // extract the request type
  HttpRequest::HttpRequestType type = request->requestType();
  string contentType = StringUtils::tolower(StringUtils::trim(request->header("content-type")));

  if (type != HttpRequest::HTTP_REQUEST_POST || contentType != getContentType()) {
    generateNotImplemented("ILLEGAL " + BATCH_PATH);
    return HANDLER_DONE;
  }

  PB_ArangoMessage inputMessages;

  bool result = inputMessages.ParseFromArray(request->body(), request->bodySize());
  if (!result) {
    generateError(HttpResponse::BAD,
                  TRI_ERROR_HTTP_BAD_PARAMETER,
                  "invalid protobuf message");
    return HANDLER_DONE;
  }

  assert(_task);
  HttpServer* server = dynamic_cast<HttpServer*>(_task->getServer());
  assert(server);

  bool failed = false;
  bool hasAsync = false;

  // loop over the input messages once to set up the output structures without concurrency
  for (int i = 0; i < inputMessages.messages_size(); ++i) {
    _outputMessages->add_messages();

    // create a handler for each input part
    const PB_ArangoBatchMessage inputMessage = inputMessages.messages(i);
    HttpRequestProtobuf* request = new HttpRequestProtobuf(inputMessage);
    HttpHandler* handler = server->createHandler(request);

    if (!handler) {
      failed = true;
      break;
    }
    else {
      _handlers.push_back(handler);
      if (!handler->isDirect()) {
        // async handler
        ++_missingResponses;
        hasAsync = true;
      }
    }
  }

  if (failed) {
    // TODO: handle error!
    std::cout << "SOMETHING FAILED--------------------------------------------------------------------------\n";
    return Handler::HANDLER_DONE;
  }

  try {
  // now loop again with all output structures set up
  for (int i = 0; i < inputMessages.messages_size(); ++i) {
    const PB_ArangoBatchMessage inputMessage = inputMessages.messages(i);

    HttpHandler* handler = _handlers[i];

    assert(handler);

    if (handler->isDirect()) {
      // execute handler directly
      Handler::status_e status = Handler::HANDLER_FAILED;

      try {
        status = handler->execute();
      }
      catch (...) {
        // TODO
      }

      if (status != Handler::HANDLER_REQUEUE) {
        addResponse(handler);
      }
    }
    else {
      // execute handler via dispatcher
      Scheduler* scheduler = server->getScheduler();
      Dispatcher* dispatcher = server->getDispatcher();

      Job* job = handler->createJob(scheduler, dispatcher, _task);

      GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler>* generalJob =
        dynamic_cast<GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler> * >(job);
      generalJob->attachObserver(this);

      dispatcher->addJob(job);
    }
  }
  }
  catch (...) {
    std::cout << "SOMETHING WENT WRONG - EXCEPTION\n";
  }

  if (!hasAsync) {
    _reallyDone = 1;
    return Handler::HANDLER_DONE;
  }

  // we have async jobs
  return Handler::HANDLER_DETACH;
}

////////////////////////////////////////////////////////////////////////////////
/// {@inheritDoc}
////////////////////////////////////////////////////////////////////////////////

bool RestBatchHandler::handleAsync () {
  if (_reallyDone) {
    assembleResponse();
    toServerJob(_job)->setDone();
    return HttpHandler::handleAsync();
  }

  return true;
}

////////////////////////////////////////////////////////////////////////////////
/// notification routine called by async sub jobs
////////////////////////////////////////////////////////////////////////////////

void RestBatchHandler::notify (Job* job, const Job::notification_e type) {
  if (type != Job::JOB_CLEANUP) {
    return;
  }

  assert(_reallyDone == 0);
  HttpHandler* handler = toServerJob(job)->getHandler();
  addResponse(handler);

  if (--_missingResponses == 0) {
    _reallyDone = 1;

    // signal to the task that we are done
    GeneralAsyncCommTask<HttpServer, HttpHandlerFactory, HttpCommTask>* atask =
      dynamic_cast<GeneralAsyncCommTask<HttpServer, HttpHandlerFactory, HttpCommTask>*>(_task);
    atask->signal();
  }
}

////////////////////////////////////////////////////////////////////////////////
/// @brief add a single handler response to the output array
////////////////////////////////////////////////////////////////////////////////

void RestBatchHandler::addResponse (HttpHandler* handler) {
  for (size_t i = 0; i < _handlers.size(); ++i) {
    if (_handlers[i] == handler) {
      // avoid concurrent modifications to the structure
      MUTEX_LOCKER(_handlerLock);
      PB_ArangoBatchMessage* batch = _outputMessages->mutable_messages(i);

      handler->getResponse()->write(batch);
      // delete the handler
      _task->getServer()->destroyHandler(handler);
      _handlers[i] = 0;
      return;
    }
  }

  // handler not found
  LOGGER_WARNING << "handler not found. this should not happen.";
}

////////////////////////////////////////////////////////////////////////////////
/// @brief create an overall protobuf response from the array of responses
////////////////////////////////////////////////////////////////////////////////

void RestBatchHandler::assembleResponse () {
  assert(_missingResponses == 0);

  response = new HttpResponse(HttpResponse::OK);
  response->setContentType(getContentType());

  string data;
  if (!_outputMessages->SerializeToString(&data)) {
    // TODO
  }
  response->body().appendText(data);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief convert a Job* to a GeneralServerJob*
////////////////////////////////////////////////////////////////////////////////

GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler>* RestBatchHandler::toServerJob(Job* job) {
  return dynamic_cast<GeneralServerJob<HttpServer, HttpHandlerFactory::GeneralHandler> * >(job);
}

////////////////////////////////////////////////////////////////////////////////
/// @brief return the required content type string
////////////////////////////////////////////////////////////////////////////////

string const& RestBatchHandler::getContentType () {
  static string const contentType = "application/x-protobuf";

  return contentType;
}

////////////////////////////////////////////////////////////////////////////////
/// @}
////////////////////////////////////////////////////////////////////////////////

// Local Variables:
// mode: outline-minor
// outline-regexp: "^\\(/// @brief\\|/// {@inheritDoc}\\|/// @addtogroup\\|// --SECTION--\\|/// @\\}\\)"
// End:
<|endoftext|>


<commit_before>// Filename: decompressor.cxx
// Created by:  mike (09Jan97)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
//
// All use of this software is subject to the terms of the Panda 3d
// Software license.  You should have received a copy of this license
// along with this source code; you will also find a current copy of
// the license at http://www.panda3d.org/license.txt .
//
// To contact the maintainers of this program write to
// panda3d@yahoogroups.com .
//
////////////////////////////////////////////////////////////////////

// This file is compiled only if we have zlib installed.

#include "config_downloader.h"

#include "error_utils.h"
#include "filename.h"
#include "buffer.h"
#include "zStream.h"

#include "decompressor.h"

#include <stdio.h>
#include <errno.h>

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::Constructor
//       Access: Public
//  Description:
////////////////////////////////////////////////////////////////////
Decompressor::
Decompressor() {
  _source = NULL;
  _decompress = NULL;
  _dest = NULL;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::Destructor
//       Access: Public
//  Description:
////////////////////////////////////////////////////////////////////
Decompressor::
~Decompressor() {
  cleanup();
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::initiate
//       Access: Public
//  Description: Begins a background decompression of the named file
//               (whose filename must end in ".pz") to a new file
//               without the .pz extension.  The source file is
//               removed after successful completion.
////////////////////////////////////////////////////////////////////
int Decompressor::
initiate(const Filename &source_file) {
  string extension = source_file.get_extension();
  if (extension == "pz") {
    Filename dest_file = source_file;
    dest_file = source_file.get_fullpath_wo_extension();
    return initiate(source_file, dest_file);
  }

  if (downloader_cat.is_debug()) {
    downloader_cat.debug()
      << "Unknown file extension for decompressor: ."
      << extension << endl;
  }
  return EU_error_abort;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::initiate
//       Access: Public
//  Description: Begins a background decompression from the named
//               source file to the named destination file.  The
//               source file is removed after successful completion.
////////////////////////////////////////////////////////////////////
int Decompressor::
initiate(const Filename &source_file, const Filename &dest_file) {
  cleanup();

  // Open source file
  _source_filename = Filename(source_file);
  _source_filename.set_binary();

  ifstream *source_fstream = new ifstream;
  _source = source_fstream;
  if (!_source_filename.open_read(*source_fstream)) {
    downloader_cat.error()
      << "Unable to read " << _source_filename << "\n";
    return get_write_error();
  }

  // Determine source file length
  source_fstream->seekg(0, ios::end);
  _source_length = source_fstream->tellg();
  if (_source_length == 0) {
    downloader_cat.warning()
      << "Zero length file: " << source_file << "\n";
    return EU_error_file_empty;
  }
  source_fstream->seekg(0, ios::beg);

  // Open destination file
  Filename dest_filename(dest_file);
  dest_filename.set_binary();

  ofstream *dest_fstream = new ofstream;
  _dest = dest_fstream;
  if (dest_filename.exists()) {
    downloader_cat.info()
      << dest_filename << " already exists, removing.\n";
    if (!dest_filename.unlink()) {
      downloader_cat.error()
        << "Unable to remove old " << dest_filename << "\n";
      return get_write_error();
    }
  } else {
    if (downloader_cat.is_debug()) {
      downloader_cat.debug()
        << dest_filename << " does not already exist.\n";
    }
  }
  if (!dest_filename.open_write(*dest_fstream)) {
    downloader_cat.error()
      << "Unable to write to " << dest_filename << "\n";
    return get_write_error();
  }

  // Now create the decompressor stream.
  _decompress = new IDecompressStream(_source, false);
  return EU_success;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::run
//       Access: Public
//  Description: Called each frame to do the next bit of work in the
//               background task.  Returns EU_ok if a chunk is
//               completed but there is more to go, or EU_success when
//               we're all done.  Any other return value indicates an
//               error.
////////////////////////////////////////////////////////////////////
int Decompressor::
run() {
  if (_decompress == (istream *)NULL) {
    // Hmm, we were already done.
    return EU_success;
  }

  // Read a bunch of characters from the decompress stream, but no
  // more than decompressor_buffer_size.
  int count = 0;
  int ch = _decompress->get();
  while (!_decompress->eof() && !_decompress->fail()) {
    _dest->put(ch);
    if (++count >= decompressor_buffer_size) {
      // That's enough for now.
      return EU_ok;
    }

    ch = _decompress->get();
  }

  // All done!
  cleanup();
  _source_filename.unlink();
  return EU_success;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::decompress
//       Access: Public
//  Description: Performs a foreground decompression of the named
//               file; does not return until the decompression is
//               complete.
////////////////////////////////////////////////////////////////////
bool Decompressor::
decompress(const Filename &source_file) {
  int ret = initiate(source_file);
  if (ret < 0)
    return false;

  int ch = _decompress->get();
  while (!_decompress->eof() && !_decompress->fail()) {
    _dest->put(ch);
    ch = _decompress->get();
  }

  cleanup();
  _source_filename.unlink();
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::decompress
//       Access: Public
//  Description: Does an in-memory decompression of the indicated
//               Ramfile.  The decompressed contents are written back
//               into the same Ramfile on completion.
////////////////////////////////////////////////////////////////////
bool Decompressor::
decompress(Ramfile &source_and_dest_file) {
  istringstream source(source_and_dest_file._data);
  ostringstream dest;

  IDecompressStream decompress(&source, false);

  int ch = decompress.get();
  while (!decompress.eof() && !decompress.fail()) {
    dest.put(ch);
    ch = decompress.get();
  }

  source_and_dest_file._pos = 0;
  source_and_dest_file._data = dest.str();
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::get_progress
//       Access: Public
//  Description: Returns the ratio through the decompression step
//               in the background.
////////////////////////////////////////////////////////////////////
float Decompressor::
get_progress() const {
  if (_decompress == (istream *)NULL) {
    // Hmm, we were already done.
    return 1.0f;
  }

  nassertr(_source_length > 0, 0.0);
  size_t source_pos = _source->tellg();

  // We stop the scale at 0.99 because there may be a little bit more
  // to do even after the decompressor has read all of the source.
  return (0.99f * (float)source_pos / (float)_source_length);
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::cleanup
//       Access: Private
//  Description: Called to reset a previous decompressor state and
//               clean up properly.
////////////////////////////////////////////////////////////////////
void Decompressor::
cleanup() {
  if (_source != (istream *)NULL) {
    delete _source;
    _source = NULL;
  }
  if (_dest != (ostream *)NULL) {
    delete _dest;
    _dest = NULL;
  }
  if (_decompress != (istream *)NULL) {
    delete _decompress;
    _decompress = NULL;
  }
}
<commit_msg>keep_temporary_files<commit_after>// Filename: decompressor.cxx
// Created by:  mike (09Jan97)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) 2001, Disney Enterprises, Inc.  All rights reserved
//
// All use of this software is subject to the terms of the Panda 3d
// Software license.  You should have received a copy of this license
// along with this source code; you will also find a current copy of
// the license at http://www.panda3d.org/license.txt .
//
// To contact the maintainers of this program write to
// panda3d@yahoogroups.com .
//
////////////////////////////////////////////////////////////////////

// This file is compiled only if we have zlib installed.

#include "config_downloader.h"

#include "error_utils.h"
#include "filename.h"
#include "buffer.h"
#include "zStream.h"
#include "config_express.h"

#include "decompressor.h"

#include <stdio.h>
#include <errno.h>

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::Constructor
//       Access: Public
//  Description:
////////////////////////////////////////////////////////////////////
Decompressor::
Decompressor() {
  _source = NULL;
  _decompress = NULL;
  _dest = NULL;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::Destructor
//       Access: Public
//  Description:
////////////////////////////////////////////////////////////////////
Decompressor::
~Decompressor() {
  cleanup();
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::initiate
//       Access: Public
//  Description: Begins a background decompression of the named file
//               (whose filename must end in ".pz") to a new file
//               without the .pz extension.  The source file is
//               removed after successful completion.
////////////////////////////////////////////////////////////////////
int Decompressor::
initiate(const Filename &source_file) {
  string extension = source_file.get_extension();
  if (extension == "pz") {
    Filename dest_file = source_file;
    dest_file = source_file.get_fullpath_wo_extension();
    return initiate(source_file, dest_file);
  }

  if (downloader_cat.is_debug()) {
    downloader_cat.debug()
      << "Unknown file extension for decompressor: ."
      << extension << endl;
  }
  return EU_error_abort;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::initiate
//       Access: Public
//  Description: Begins a background decompression from the named
//               source file to the named destination file.  The
//               source file is removed after successful completion.
////////////////////////////////////////////////////////////////////
int Decompressor::
initiate(const Filename &source_file, const Filename &dest_file) {
  cleanup();

  // Open source file
  _source_filename = Filename(source_file);
  _source_filename.set_binary();

  ifstream *source_fstream = new ifstream;
  _source = source_fstream;
  if (!_source_filename.open_read(*source_fstream)) {
    downloader_cat.error()
      << "Unable to read " << _source_filename << "\n";
    return get_write_error();
  }

  // Determine source file length
  source_fstream->seekg(0, ios::end);
  _source_length = source_fstream->tellg();
  if (_source_length == 0) {
    downloader_cat.warning()
      << "Zero length file: " << source_file << "\n";
    return EU_error_file_empty;
  }
  source_fstream->seekg(0, ios::beg);

  // Open destination file
  Filename dest_filename(dest_file);
  dest_filename.set_binary();

  ofstream *dest_fstream = new ofstream;
  _dest = dest_fstream;
  if (dest_filename.exists()) {
    downloader_cat.info()
      << dest_filename << " already exists, removing.\n";
    if (!dest_filename.unlink()) {
      downloader_cat.error()
        << "Unable to remove old " << dest_filename << "\n";
      return get_write_error();
    }
  } else {
    if (downloader_cat.is_debug()) {
      downloader_cat.debug()
        << dest_filename << " does not already exist.\n";
    }
  }
  if (!dest_filename.open_write(*dest_fstream)) {
    downloader_cat.error()
      << "Unable to write to " << dest_filename << "\n";
    return get_write_error();
  }

  // Now create the decompressor stream.
  _decompress = new IDecompressStream(_source, false);
  return EU_success;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::run
//       Access: Public
//  Description: Called each frame to do the next bit of work in the
//               background task.  Returns EU_ok if a chunk is
//               completed but there is more to go, or EU_success when
//               we're all done.  Any other return value indicates an
//               error.
////////////////////////////////////////////////////////////////////
int Decompressor::
run() {
  if (_decompress == (istream *)NULL) {
    // Hmm, we were already done.
    return EU_success;
  }

  // Read a bunch of characters from the decompress stream, but no
  // more than decompressor_buffer_size.
  int count = 0;
  int ch = _decompress->get();
  while (!_decompress->eof() && !_decompress->fail()) {
    _dest->put(ch);
    if (++count >= decompressor_buffer_size) {
      // That's enough for now.
      return EU_ok;
    }

    ch = _decompress->get();
  }

  // All done!
  cleanup();
  if (!keep_temporary_files) {
    _source_filename.unlink();
  }
  return EU_success;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::decompress
//       Access: Public
//  Description: Performs a foreground decompression of the named
//               file; does not return until the decompression is
//               complete.
////////////////////////////////////////////////////////////////////
bool Decompressor::
decompress(const Filename &source_file) {
  int ret = initiate(source_file);
  if (ret < 0)
    return false;

  int ch = _decompress->get();
  while (!_decompress->eof() && !_decompress->fail()) {
    _dest->put(ch);
    ch = _decompress->get();
  }

  cleanup();
  if (!keep_temporary_files) {
    _source_filename.unlink();
  }
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::decompress
//       Access: Public
//  Description: Does an in-memory decompression of the indicated
//               Ramfile.  The decompressed contents are written back
//               into the same Ramfile on completion.
////////////////////////////////////////////////////////////////////
bool Decompressor::
decompress(Ramfile &source_and_dest_file) {
  istringstream source(source_and_dest_file._data);
  ostringstream dest;

  IDecompressStream decompress(&source, false);

  int ch = decompress.get();
  while (!decompress.eof() && !decompress.fail()) {
    dest.put(ch);
    ch = decompress.get();
  }

  source_and_dest_file._pos = 0;
  source_and_dest_file._data = dest.str();
  return true;
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::get_progress
//       Access: Public
//  Description: Returns the ratio through the decompression step
//               in the background.
////////////////////////////////////////////////////////////////////
float Decompressor::
get_progress() const {
  if (_decompress == (istream *)NULL) {
    // Hmm, we were already done.
    return 1.0f;
  }

  nassertr(_source_length > 0, 0.0);
  size_t source_pos = _source->tellg();

  // We stop the scale at 0.99 because there may be a little bit more
  // to do even after the decompressor has read all of the source.
  return (0.99f * (float)source_pos / (float)_source_length);
}

////////////////////////////////////////////////////////////////////
//     Function: Decompressor::cleanup
//       Access: Private
//  Description: Called to reset a previous decompressor state and
//               clean up properly.
////////////////////////////////////////////////////////////////////
void Decompressor::
cleanup() {
  if (_source != (istream *)NULL) {
    delete _source;
    _source = NULL;
  }
  if (_dest != (ostream *)NULL) {
    delete _dest;
    _dest = NULL;
  }
  if (_decompress != (istream *)NULL) {
    delete _decompress;
    _decompress = NULL;
  }
}
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2015, Nagoya University
 *  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 Autoware 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 HOLDER 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 <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

#include "ros/ros.h"
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CompressedImage.h>
#include "cv_tracker/image_obj_ranged.h"
#include <math.h>
#include <float.h>
#define NO_DATA 0

static char window_name[] = "image_d_viewer";
//for imageCallback
static cv_bridge::CvImagePtr cv_image;
static IplImage temp;
static IplImage *image;
static double ratio = 1;	//resize ratio

static cv_tracker::image_obj_ranged car_fused_objects;
static cv_tracker::image_obj_ranged pedestrian_fused_objects;

static const int OBJ_RECT_THICKNESS = 3;
static void showImage();

/* check whether floating value x is nearly 0 or not */
static inline bool isNearlyNODATA(float x)
{
  float abs_x  = (float)fabs(x);
  const int rangeScale = 100;
  return(abs_x < FLT_MIN*rangeScale);
}

void showRects(IplImage *Image,
               std::vector<cv_tracker::image_rect_ranged> objects,
               double ratio,
               CvScalar col)
{
    unsigned int object_num = objects.size();
    for(unsigned int i = 0; i < object_num; i++)
    {
        if (!isNearlyNODATA(objects.at(i).range))
        {
            CvPoint p1=cvPoint(objects.at(i).rect.x, objects.at(i).rect.y);
            CvPoint p2=cvPoint(objects.at(i).rect.x + objects.at(i).rect.width, objects.at(i).rect.y + objects.at(i).rect.height);
            cvRectangle(Image,p1,p2,col,OBJ_RECT_THICKNESS);
        }
    }
}

static void obj_carCallback(const cv_tracker::image_obj_ranged& fused_objects)
{
    if(image == NULL){
      return;
    }
    car_fused_objects = fused_objects;
    showImage();
}

static void obj_personCallback(const cv_tracker::image_obj_ranged& fused_objects)
{
    if(image == NULL){
      return;
    }
    pedestrian_fused_objects = fused_objects;
    showImage();
}

static void imageCallback(const sensor_msgs::Image& image_source)
{
    cv_image = cv_bridge::toCvCopy(image_source, sensor_msgs::image_encodings::BGR8);
    temp = cv_image->image;
    image = &temp;
    showImage();
}

static void showImage()
{
    IplImage* image_clone = cvCloneImage(image);
    char distance_string[32];
    CvFont dfont;
    float hscale      = 0.7f;
    float vscale      = 0.7f;
    float italicscale = 0.0f;
    int  thickness    = 1;

    std::string objectLabel;
    CvFont      dfont_label;
    float       hscale_label = 0.5f;
    float       vscale_label = 0.5f;
    CvSize      text_size;
    int         baseline     = 0;

    cvInitFont(&dfont_label, CV_FONT_HERSHEY_COMPLEX, hscale_label, vscale_label, italicscale, thickness, CV_AA);
    objectLabel = car_fused_objects.type;
    cvGetTextSize(objectLabel.data(),
                  &dfont_label,
                  &text_size,
                  &baseline);

    /*
     * Plot obstacle frame
     */
    showRects(image_clone,
              car_fused_objects.obj,
              ratio,
              cvScalar(255.0,255.0,0.0));
    showRects(image_clone,
              pedestrian_fused_objects.obj,
              ratio,
              cvScalar(0.0,255.0,0.0));

    /*
     * Plot car distance data on image
     */
    for (unsigned int i = 0; i < car_fused_objects.obj.size(); i++) {
      if(!isNearlyNODATA(car_fused_objects.obj.at(i).range)) {
          int rect_x      = car_fused_objects.obj.at(i).rect.x;
          int rect_y      = car_fused_objects.obj.at(i).rect.y;
          int rect_width  = car_fused_objects.obj.at(i).rect.width;
          int rect_height = car_fused_objects.obj.at(i).rect.height;
          float range     = car_fused_objects.obj.at(i).range;

          /* put label */
          CvPoint labelOrg = cvPoint(rect_x - OBJ_RECT_THICKNESS,
                                     rect_y - baseline - OBJ_RECT_THICKNESS);
          cvRectangle(image_clone,
                      cvPoint(labelOrg.x + 0, labelOrg.y + baseline),
                      cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height),
                      CV_RGB(0, 0, 0), // label background color is black
                      -1, 8, 0
                      );
          cvPutText(image_clone,
                    objectLabel.data(),
                    labelOrg,
                    &dfont_label,
                    CV_RGB(255, 255, 255) // label text color is white
                    );

          /* put distance data */
            cvRectangle(image_clone,
                        cv::Point(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 45),
                                  rect_y + rect_height + 5),
                        cv::Point(rect_x + (rect_width/2) + (((int)log10(range/100)+1) * 8 + 38),
                                  rect_y + rect_height + 30),
                        cv::Scalar(255,255,255), -1);
            cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX , hscale, vscale, italicscale, thickness, CV_AA);
            sprintf(distance_string, "%.2f m", range / 100); //unit of length is meter
            cvPutText(image_clone,
                      distance_string,
                      cvPoint(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 40),
                              rect_y + rect_height + 25),
                      &dfont,
                      CV_RGB(255, 0, 0));
        }
    }

    objectLabel = pedestrian_fused_objects.type;
    cvGetTextSize(objectLabel.data(),
                  &dfont_label,
                  &text_size,
                  &baseline);

    /*
     * Plot pedestrian distance data on image
     */
    for (unsigned int i = 0; i < pedestrian_fused_objects.obj.size(); i++) {
      if(!isNearlyNODATA(pedestrian_fused_objects.obj.at(i).range)) {
          int rect_x      = pedestrian_fused_objects.obj.at(i).rect.x;
          int rect_y      = pedestrian_fused_objects.obj.at(i).rect.y;
          int rect_width  = pedestrian_fused_objects.obj.at(i).rect.width;
          int rect_height = pedestrian_fused_objects.obj.at(i).rect.height;
          float range     = pedestrian_fused_objects.obj.at(i).range;

          /* put label */
          CvPoint labelOrg = cvPoint(rect_x - OBJ_RECT_THICKNESS,
                                     rect_y - baseline - OBJ_RECT_THICKNESS);
          cvRectangle(image_clone,
                      cvPoint(labelOrg.x + 0, labelOrg.y + baseline),
                      cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height),
                      CV_RGB(0, 0, 0), // label background color is black
                      -1, 8, 0
                      );
          cvPutText(image_clone,
                    objectLabel.data(),
                    labelOrg,
                    &dfont_label,
                    CV_RGB(255, 255, 255) // label text color is white
                    );

          /* put distance data */
            cvRectangle(image_clone,
                        cv::Point(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 45),
                                  rect_y + rect_height + 5),
                        cv::Point(rect_x + (rect_width/2) + (((int)log10(range/100)+1) * 8 + 38),
                                  rect_y + rect_height + 30),
                        cv::Scalar(255,255,255), -1);
            cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX , hscale, vscale, italicscale, thickness, CV_AA);
            sprintf(distance_string, "%.2f m", range / 100); //unit of length is meter
            cvPutText(image_clone,
                      distance_string,
                      cvPoint(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 40),
                              rect_y + rect_height + 25),
                      &dfont,
                      CV_RGB(255, 0, 0));
        }
    }

    /*
     * Show image
     */
    if (cvGetWindowHandle(window_name) != NULL) // Guard not to write destroyed window by using close button on the window
      {
        cvShowImage(window_name, image_clone);
        cvWaitKey(2);
      }
    cvReleaseImage(&image_clone);
}

int main(int argc, char **argv)
{
   /**
    * The ros::init() function needs to see argc and argv so that it can perform
    * any ROS arguments and name remapping that were provided at the command line. For programmatic
    * remappings you can use a different version of init() which takes remappings
    * directly, but for most command-line programs, passing argc and argv is the easiest
    * way to do it.  The third argument to init() is the name of the node.
    *
    * You must call one of the versions of ros::init() before using any other
    * part of the ROS system.
    */

    cvNamedWindow(window_name, 2);
    cvStartWindowThread();
    image = NULL;
    car_fused_objects.obj.clear();
    pedestrian_fused_objects.obj.clear();

    ros::init(argc, argv, "image_d_viewer");

    /**
     * NodeHandle is the main access point to communications with the ROS system.
     * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle n;

    /**
     * The subscribe() call is how you tell ROS that you want to receive messages
     * on a given topic.  This invokes a call to the ROS
     * master node, which keeps a registry of who is publishing and who
     * is subscribing.  Messages are passed to a callback function, here
     * called Callback.  subscribe() returns a Subscriber object that you
     * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
     * object go out of scope, this callback will automatically be unsubscribed from
     * this topic.
     *
     * The second parameter to the subscribe() function is the size of the message
     * queue.  If messages are arriving faster than they are being processed, this
     * is the number of messages that will be buffered up before beginning to throw
     * away the oldest ones.
     */

    ros::Subscriber image_sub = n.subscribe("/image_raw", 1, imageCallback);
    ros::Subscriber obj_car_sub = n.subscribe("/obj_car/image_obj_ranged", 1, obj_carCallback);
    ros::Subscriber obj_person_sub = n.subscribe("/obj_car/image_obj_ranged", 1, obj_personCallback);

    /**
     * ros::spin() will enter a loop, pumping callbacks.  With this version, all
     * callbacks will be called from within this thread (the main one).  ros::spin()
     * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
     */
    ros::spin();
    cvDestroyWindow(window_name);

    return 0;
}
<commit_msg>Modified image_d_viewe to make subscribed topic name being selectable<commit_after>/*
 *  Copyright (c) 2015, Nagoya University
 *  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 Autoware 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 HOLDER 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 <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

#include "ros/ros.h"
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CompressedImage.h>
#include "cv_tracker/image_obj_ranged.h"
#include <math.h>
#include <float.h>
#define NO_DATA 0

static char window_name_base[] = "image_d_viewer";
static std::string window_name;
//for imageCallback
static cv_bridge::CvImagePtr cv_image;
static IplImage temp;
static IplImage *image;
static double ratio = 1;	//resize ratio

static cv_tracker::image_obj_ranged car_fused_objects;
static cv_tracker::image_obj_ranged pedestrian_fused_objects;

static const int OBJ_RECT_THICKNESS = 3;
static void showImage();

/* check whether floating value x is nearly 0 or not */
static inline bool isNearlyNODATA(float x)
{
  float abs_x  = (float)fabs(x);
  const int rangeScale = 100;
  return(abs_x < FLT_MIN*rangeScale);
}

void showRects(IplImage *Image,
               std::vector<cv_tracker::image_rect_ranged> objects,
               double ratio,
               CvScalar col)
{
    unsigned int object_num = objects.size();
    for(unsigned int i = 0; i < object_num; i++)
    {
        if (!isNearlyNODATA(objects.at(i).range))
        {
            CvPoint p1=cvPoint(objects.at(i).rect.x, objects.at(i).rect.y);
            CvPoint p2=cvPoint(objects.at(i).rect.x + objects.at(i).rect.width, objects.at(i).rect.y + objects.at(i).rect.height);
            cvRectangle(Image,p1,p2,col,OBJ_RECT_THICKNESS);
        }
    }
}

static void obj_carCallback(const cv_tracker::image_obj_ranged& fused_objects)
{
    if(image == NULL){
      return;
    }
    car_fused_objects = fused_objects;
    showImage();
}

static void obj_personCallback(const cv_tracker::image_obj_ranged& fused_objects)
{
    if(image == NULL){
      return;
    }
    pedestrian_fused_objects = fused_objects;
    showImage();
}

static void imageCallback(const sensor_msgs::Image& image_source)
{
    cv_image = cv_bridge::toCvCopy(image_source, sensor_msgs::image_encodings::BGR8);
    temp = cv_image->image;
    image = &temp;
    showImage();
}

static void showImage()
{
    IplImage* image_clone = cvCloneImage(image);
    char distance_string[32];
    CvFont dfont;
    float hscale      = 0.7f;
    float vscale      = 0.7f;
    float italicscale = 0.0f;
    int  thickness    = 1;

    std::string objectLabel;
    CvFont      dfont_label;
    float       hscale_label = 0.5f;
    float       vscale_label = 0.5f;
    CvSize      text_size;
    int         baseline     = 0;

    cvInitFont(&dfont_label, CV_FONT_HERSHEY_COMPLEX, hscale_label, vscale_label, italicscale, thickness, CV_AA);
    objectLabel = car_fused_objects.type;
    cvGetTextSize(objectLabel.data(),
                  &dfont_label,
                  &text_size,
                  &baseline);

    /*
     * Plot obstacle frame
     */
    showRects(image_clone,
              car_fused_objects.obj,
              ratio,
              cvScalar(255.0,255.0,0.0));
    showRects(image_clone,
              pedestrian_fused_objects.obj,
              ratio,
              cvScalar(0.0,255.0,0.0));

    /*
     * Plot car distance data on image
     */
    for (unsigned int i = 0; i < car_fused_objects.obj.size(); i++) {
      if(!isNearlyNODATA(car_fused_objects.obj.at(i).range)) {
          int rect_x      = car_fused_objects.obj.at(i).rect.x;
          int rect_y      = car_fused_objects.obj.at(i).rect.y;
          int rect_width  = car_fused_objects.obj.at(i).rect.width;
          int rect_height = car_fused_objects.obj.at(i).rect.height;
          float range     = car_fused_objects.obj.at(i).range;

          /* put label */
          CvPoint labelOrg = cvPoint(rect_x - OBJ_RECT_THICKNESS,
                                     rect_y - baseline - OBJ_RECT_THICKNESS);
          cvRectangle(image_clone,
                      cvPoint(labelOrg.x + 0, labelOrg.y + baseline),
                      cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height),
                      CV_RGB(0, 0, 0), // label background color is black
                      -1, 8, 0
                      );
          cvPutText(image_clone,
                    objectLabel.data(),
                    labelOrg,
                    &dfont_label,
                    CV_RGB(255, 255, 255) // label text color is white
                    );

          /* put distance data */
            cvRectangle(image_clone,
                        cv::Point(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 45),
                                  rect_y + rect_height + 5),
                        cv::Point(rect_x + (rect_width/2) + (((int)log10(range/100)+1) * 8 + 38),
                                  rect_y + rect_height + 30),
                        cv::Scalar(255,255,255), -1);
            cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX , hscale, vscale, italicscale, thickness, CV_AA);
            sprintf(distance_string, "%.2f m", range / 100); //unit of length is meter
            cvPutText(image_clone,
                      distance_string,
                      cvPoint(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 40),
                              rect_y + rect_height + 25),
                      &dfont,
                      CV_RGB(255, 0, 0));
        }
    }

    objectLabel = pedestrian_fused_objects.type;
    cvGetTextSize(objectLabel.data(),
                  &dfont_label,
                  &text_size,
                  &baseline);

    /*
     * Plot pedestrian distance data on image
     */
    for (unsigned int i = 0; i < pedestrian_fused_objects.obj.size(); i++) {
      if(!isNearlyNODATA(pedestrian_fused_objects.obj.at(i).range)) {
          int rect_x      = pedestrian_fused_objects.obj.at(i).rect.x;
          int rect_y      = pedestrian_fused_objects.obj.at(i).rect.y;
          int rect_width  = pedestrian_fused_objects.obj.at(i).rect.width;
          int rect_height = pedestrian_fused_objects.obj.at(i).rect.height;
          float range     = pedestrian_fused_objects.obj.at(i).range;

          /* put label */
          CvPoint labelOrg = cvPoint(rect_x - OBJ_RECT_THICKNESS,
                                     rect_y - baseline - OBJ_RECT_THICKNESS);
          cvRectangle(image_clone,
                      cvPoint(labelOrg.x + 0, labelOrg.y + baseline),
                      cvPoint(labelOrg.x + text_size.width, labelOrg.y - text_size.height),
                      CV_RGB(0, 0, 0), // label background color is black
                      -1, 8, 0
                      );
          cvPutText(image_clone,
                    objectLabel.data(),
                    labelOrg,
                    &dfont_label,
                    CV_RGB(255, 255, 255) // label text color is white
                    );

          /* put distance data */
            cvRectangle(image_clone,
                        cv::Point(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 45),
                                  rect_y + rect_height + 5),
                        cv::Point(rect_x + (rect_width/2) + (((int)log10(range/100)+1) * 8 + 38),
                                  rect_y + rect_height + 30),
                        cv::Scalar(255,255,255), -1);
            cvInitFont (&dfont, CV_FONT_HERSHEY_COMPLEX , hscale, vscale, italicscale, thickness, CV_AA);
            sprintf(distance_string, "%.2f m", range / 100); //unit of length is meter
            cvPutText(image_clone,
                      distance_string,
                      cvPoint(rect_x + (rect_width/2) - (((int)log10(range/100)+1) * 5 + 40),
                              rect_y + rect_height + 25),
                      &dfont,
                      CV_RGB(255, 0, 0));
        }
    }

    /*
     * Show image
     */
    if (cvGetWindowHandle(window_name.c_str()) != NULL) // Guard not to write destroyed window by using close button on the window
      {
        cvShowImage(window_name.c_str(), image_clone);
        cvWaitKey(2);
      }
    cvReleaseImage(&image_clone);
}

int main(int argc, char **argv)
{
   /**
    * The ros::init() function needs to see argc and argv so that it can perform
    * any ROS arguments and name remapping that were provided at the command line. For programmatic
    * remappings you can use a different version of init() which takes remappings
    * directly, but for most command-line programs, passing argc and argv is the easiest
    * way to do it.  The third argument to init() is the name of the node.
    *
    * You must call one of the versions of ros::init() before using any other
    * part of the ROS system.
    */

    ros::init(argc, argv, "image_d_viewer");

    /**
     * NodeHandle is the main access point to communications with the ROS system.
     * The first NodeHandle constructed will fully initialize this node, and the last
     * NodeHandle destructed will close down the node.
     */
    ros::NodeHandle n;
    ros::NodeHandle private_nh("~");

    /**
     * The subscribe() call is how you tell ROS that you want to receive messages
     * on a given topic.  This invokes a call to the ROS
     * master node, which keeps a registry of who is publishing and who
     * is subscribing.  Messages are passed to a callback function, here
     * called Callback.  subscribe() returns a Subscriber object that you
     * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
     * object go out of scope, this callback will automatically be unsubscribed from
     * this topic.
     *
     * The second parameter to the subscribe() function is the size of the message
     * queue.  If messages are arriving faster than they are being processed, this
     * is the number of messages that will be buffered up before beginning to throw
     * away the oldest ones.
     */
    std::string image_topic;
    std::string car_topic;
    std::string person_topic;

    if (!private_nh.getParam("image_topic", image_topic)) {
      image_topic = "/image_raw";
    }

    if (!private_nh.getParam("car_topic", car_topic)) {
      car_topic = "/obj_car/image_obj_ranged";
    }

    if (!private_nh.getParam("person_topic", person_topic)) {
      person_topic = "/obj_person/image_obj_ranged";
    }

    std::string name_space_str = ros::this_node::getNamespace();
    if (name_space_str != "/") {
      window_name = std::string(window_name_base) + " (" + ros::this_node::getNamespace() + ")";
    }
    else {
      window_name = std::string(window_name_base);
    }
    cvNamedWindow(window_name.c_str(), 2);
    cvStartWindowThread();
    image = NULL;
    car_fused_objects.obj.clear();
    pedestrian_fused_objects.obj.clear();

    ros::Subscriber image_sub = n.subscribe(image_topic, 1, imageCallback);
    ros::Subscriber obj_car_sub = n.subscribe(car_topic, 1, obj_carCallback);
    ros::Subscriber obj_person_sub = n.subscribe(person_topic, 1, obj_personCallback);

    /**
     * ros::spin() will enter a loop, pumping callbacks.  With this version, all
     * callbacks will be called from within this thread (the main one).  ros::spin()
     * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
     */
    ros::spin();
    cvDestroyWindow(window_name.c_str());

    return 0;
}
<|endoftext|>