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

#include <windows.h>
#include <tchar.h>
#include "core/strings/unicode.h"
#include "math/mMath.h"
#include "windowManager/win32/win32Window.h"
#include "windowManager/win32/win32WindowMgr.h"
#include "windowManager/win32/winDispatch.h"
#include "windowManager/win32/win32CursorController.h"
#include "platform/platformInput.h"
#include <zmouse.h>

static struct { U32 id; LPTSTR resourceID; } sgCursorShapeMap[]=
{
   { PlatformCursorController::curArrow,       IDC_ARROW },
   { PlatformCursorController::curWait,        IDC_WAIT },
   { PlatformCursorController::curPlus,        IDC_CROSS },
   { PlatformCursorController::curResizeVert,  IDC_SIZEWE },
   { PlatformCursorController::curResizeHorz,  IDC_SIZENS },
   { PlatformCursorController::curResizeAll,   IDC_SIZEALL },
   { PlatformCursorController::curIBeam,       IDC_IBEAM },
   { PlatformCursorController::curResizeNESW,  IDC_SIZENESW },
   { PlatformCursorController::curResizeNWSE,  IDC_SIZENWSE },
   { PlatformCursorController::curHand,        IDC_HAND },
   { 0,                             0 },
};

//static const EnumTable::Enums curManagerShapesEnums[] =
//{
//   { Win32CursorController::curArrow, "Arrow" },
//   { Win32CursorController::curWait, "Wait" },
//   { Win32CursorController::curPlus, "Plus" },
//   { Win32CursorController::curResizeVert, "ResizeVert" },
//   { Win32CursorController::curResizeHorz, "ResizeHorz" },
//   { Win32CursorController::curResizeAll, "ResizeAll" },
//   { Win32CursorController::curIBeam, "ibeam" },
//   { Win32CursorController::curResizeNESW, "ResizeNESW" },
//   { Win32CursorController::curResizeNWSE, "ResizeNWSE" },
//};
//
//static const EnumTable gCurManagerShapesTable(8, &curManagerShapesEnums[0]);

// CodeReview I've duplicated this 'cache' trick for system settings
// because they're unlikely to change and calling into the OS for values
// repeatedly is just silly to begin with.  [6/29/2007 justind]
U32 Win32CursorController::getDoubleClickTime()
{
   static S32 sPlatWinDoubleClicktime = -1;
   if( sPlatWinDoubleClicktime == -1 )
      sPlatWinDoubleClicktime = GetDoubleClickTime();
   return sPlatWinDoubleClicktime;
}
S32 Win32CursorController::getDoubleClickWidth()
{
   static S32 sPlatWinDoubleClickwidth = -1;
   if( sPlatWinDoubleClickwidth == -1 )
      sPlatWinDoubleClickwidth = GetSystemMetrics(SM_CXDOUBLECLK);
   return sPlatWinDoubleClickwidth;
}
S32 Win32CursorController::getDoubleClickHeight()
{
   static S32 sPlatWinDoubleClickheight = -1;
   if( sPlatWinDoubleClickheight == -1 )
      sPlatWinDoubleClickheight = GetSystemMetrics(SM_CYDOUBLECLK);
   return sPlatWinDoubleClickheight;
}

void Win32CursorController::setCursorPosition( S32 x, S32 y )
{
   ::SetCursorPos(x, y);
}

void Win32CursorController::getCursorPosition( Point2I &point )
{
   POINT rPoint;
   ::GetCursorPos( &rPoint );

   // Return
   point.x = rPoint.x;
   point.y = rPoint.y;
}

void Win32CursorController::setCursorVisible( bool visible )
{
   if( visible )
      ShowCursor( true );
   else
      while( ShowCursor(false) >= 0 );
}

bool Win32CursorController::isCursorVisible()
{
   CURSORINFO rCursorInfo;
   rCursorInfo.cbSize = sizeof(CURSORINFO);
   if( !GetCursorInfo( &rCursorInfo ) )
   {
      //DWORD error = GetLastError();
      return false;
   }

   // rCursorInfo.flags values :
   // 0 == Cursor is hidden
   // CURSOR_SHOWING == cursor is visible
   return (bool)(rCursorInfo.flags == CURSOR_SHOWING);
}

void Win32CursorController::setCursorShape(U32 cursorID)
{
   LPTSTR resourceID = NULL;

   for(S32 i = 0;sgCursorShapeMap[i].resourceID != NULL;++i)
   {
      if(cursorID == sgCursorShapeMap[i].id)
      {
         resourceID = sgCursorShapeMap[i].resourceID;
         break;
      }
   }

   if(resourceID == NULL)
      return;

   HCURSOR cur = LoadCursor(NULL, resourceID);
   if(cur)
      SetCursor(cur);
}

static HCURSOR gCursorShape = NULL;
void Win32CursorController::setCursorShape( const UTF8 *fileName, bool reload )
{
#ifdef UNICODE
   const UTF16 *lFileName = createUTF16string( fileName );
#else
   const UTF8  *lFileName = fileName;
#endif

   if ( !gCursorShape || reload )
      gCursorShape = LoadCursorFromFile( lFileName );

   if ( gCursorShape )
      SetCursor( gCursorShape );
}

// Console function to set the current cursor shape given the cursor shape
// name as defined in the enum above.
//ConsoleFunction( inputPushCursor, void, 2, 2, "inputPushCursor(cursor shape name)" )
//{
//   S32 val = 0;
//
//   // Find the cursor shape
//   if(argc == 2)
//   {
//      for (S32 i = 0; i < gCurManagerShapesTable.size; i++)
//      {
//         if (! dStricmp(argv[1], gCurManagerShapesTable.table[i].label))
//         {
//            val = gCurManagerShapesTable.table[i].index;
//            break;
//         }
//      }
//   }
//
//   // Now set it
//   Win32CursorController* cm = Input::getCursorManager();
//   if(cm)
//   {
//      cm->pushCursor(val);
//   }
//}
//// Function to pop the current cursor shape
//ConsoleFunction( inputPopCursor, void, 1, 1, "inputPopCursor()" )
//{
//   argc;
//   argv;
//
//   Win32CursorController* cm = Input::getCursorManager();
//   if(cm)
//   {
//      cm->popCursor();
//   }
//}
<commit_msg>Fix mem leak discovered during previous commit<commit_after>//-----------------------------------------------------------------------------
// Copyright (c) 2012 GarageGames, LLC
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
//-----------------------------------------------------------------------------

#include <windows.h>
#include <tchar.h>
#include "core/strings/unicode.h"
#include "math/mMath.h"
#include "windowManager/win32/win32Window.h"
#include "windowManager/win32/win32WindowMgr.h"
#include "windowManager/win32/winDispatch.h"
#include "windowManager/win32/win32CursorController.h"
#include "platform/platformInput.h"
#include <zmouse.h>

static struct { U32 id; LPTSTR resourceID; } sgCursorShapeMap[]=
{
   { PlatformCursorController::curArrow,       IDC_ARROW },
   { PlatformCursorController::curWait,        IDC_WAIT },
   { PlatformCursorController::curPlus,        IDC_CROSS },
   { PlatformCursorController::curResizeVert,  IDC_SIZEWE },
   { PlatformCursorController::curResizeHorz,  IDC_SIZENS },
   { PlatformCursorController::curResizeAll,   IDC_SIZEALL },
   { PlatformCursorController::curIBeam,       IDC_IBEAM },
   { PlatformCursorController::curResizeNESW,  IDC_SIZENESW },
   { PlatformCursorController::curResizeNWSE,  IDC_SIZENWSE },
   { PlatformCursorController::curHand,        IDC_HAND },
   { 0,                             0 },
};

//static const EnumTable::Enums curManagerShapesEnums[] =
//{
//   { Win32CursorController::curArrow, "Arrow" },
//   { Win32CursorController::curWait, "Wait" },
//   { Win32CursorController::curPlus, "Plus" },
//   { Win32CursorController::curResizeVert, "ResizeVert" },
//   { Win32CursorController::curResizeHorz, "ResizeHorz" },
//   { Win32CursorController::curResizeAll, "ResizeAll" },
//   { Win32CursorController::curIBeam, "ibeam" },
//   { Win32CursorController::curResizeNESW, "ResizeNESW" },
//   { Win32CursorController::curResizeNWSE, "ResizeNWSE" },
//};
//
//static const EnumTable gCurManagerShapesTable(8, &curManagerShapesEnums[0]);

// CodeReview I've duplicated this 'cache' trick for system settings
// because they're unlikely to change and calling into the OS for values
// repeatedly is just silly to begin with.  [6/29/2007 justind]
U32 Win32CursorController::getDoubleClickTime()
{
   static S32 sPlatWinDoubleClicktime = -1;
   if( sPlatWinDoubleClicktime == -1 )
      sPlatWinDoubleClicktime = GetDoubleClickTime();
   return sPlatWinDoubleClicktime;
}
S32 Win32CursorController::getDoubleClickWidth()
{
   static S32 sPlatWinDoubleClickwidth = -1;
   if( sPlatWinDoubleClickwidth == -1 )
      sPlatWinDoubleClickwidth = GetSystemMetrics(SM_CXDOUBLECLK);
   return sPlatWinDoubleClickwidth;
}
S32 Win32CursorController::getDoubleClickHeight()
{
   static S32 sPlatWinDoubleClickheight = -1;
   if( sPlatWinDoubleClickheight == -1 )
      sPlatWinDoubleClickheight = GetSystemMetrics(SM_CYDOUBLECLK);
   return sPlatWinDoubleClickheight;
}

void Win32CursorController::setCursorPosition( S32 x, S32 y )
{
   ::SetCursorPos(x, y);
}

void Win32CursorController::getCursorPosition( Point2I &point )
{
   POINT rPoint;
   ::GetCursorPos( &rPoint );

   // Return
   point.x = rPoint.x;
   point.y = rPoint.y;
}

void Win32CursorController::setCursorVisible( bool visible )
{
   if( visible )
      ShowCursor( true );
   else
      while( ShowCursor(false) >= 0 );
}

bool Win32CursorController::isCursorVisible()
{
   CURSORINFO rCursorInfo;
   rCursorInfo.cbSize = sizeof(CURSORINFO);
   if( !GetCursorInfo( &rCursorInfo ) )
   {
      //DWORD error = GetLastError();
      return false;
   }

   // rCursorInfo.flags values :
   // 0 == Cursor is hidden
   // CURSOR_SHOWING == cursor is visible
   return (bool)(rCursorInfo.flags == CURSOR_SHOWING);
}

void Win32CursorController::setCursorShape(U32 cursorID)
{
   LPTSTR resourceID = NULL;

   for(S32 i = 0;sgCursorShapeMap[i].resourceID != NULL;++i)
   {
      if(cursorID == sgCursorShapeMap[i].id)
      {
         resourceID = sgCursorShapeMap[i].resourceID;
         break;
      }
   }

   if(resourceID == NULL)
      return;

   HCURSOR cur = LoadCursor(NULL, resourceID);
   if(cur)
      SetCursor(cur);
}

static HCURSOR gCursorShape = NULL;
void Win32CursorController::setCursorShape( const UTF8 *fileName, bool reload )
{
#ifdef UNICODE
   const UTF16 *lFileName = createUTF16string( fileName );
#else
   const UTF8  *lFileName = fileName;
#endif

   if ( !gCursorShape || reload )
      gCursorShape = LoadCursorFromFile( lFileName );

   if ( gCursorShape )
      SetCursor( gCursorShape );

#ifdef UNICODE
   delete[] lFileName;
#endif
}

// Console function to set the current cursor shape given the cursor shape
// name as defined in the enum above.
//ConsoleFunction( inputPushCursor, void, 2, 2, "inputPushCursor(cursor shape name)" )
//{
//   S32 val = 0;
//
//   // Find the cursor shape
//   if(argc == 2)
//   {
//      for (S32 i = 0; i < gCurManagerShapesTable.size; i++)
//      {
//         if (! dStricmp(argv[1], gCurManagerShapesTable.table[i].label))
//         {
//            val = gCurManagerShapesTable.table[i].index;
//            break;
//         }
//      }
//   }
//
//   // Now set it
//   Win32CursorController* cm = Input::getCursorManager();
//   if(cm)
//   {
//      cm->pushCursor(val);
//   }
//}
//// Function to pop the current cursor shape
//ConsoleFunction( inputPopCursor, void, 1, 1, "inputPopCursor()" )
//{
//   argc;
//   argv;
//
//   Win32CursorController* cm = Input::getCursorManager();
//   if(cm)
//   {
//      cm->popCursor();
//   }
//}
<|endoftext|>


<commit_before>#include "robot_interface/gripper.h"

#include <iostream>
#include <string>

using namespace baxter_core_msgs;
using namespace std;

Gripper::Gripper(std::string _limb, bool _use_robot) :
                 rnh(_limb), limb(_limb), use_robot(_use_robot), first_run(true), prop_set(false),
                 spinner(1), cmd_sequence(0), cmd_sender(ros::this_node::getName())
{
    if (not use_robot) return;

    // create a publisher for the gripper's commands
    pub_cmd = rnh.advertise<EndEffectorCommand>( "/robot/end_effector/" +
                                                _limb + "_gripper/command", 1);

    // create a subscriber to the gripper's properties
    sub_prop  = rnh.subscribe("/robot/end_effector/" + _limb + "_gripper/properties",
                                   SUBSCRIBER_BUFFER, &Gripper::gripperPropCb, this);

    // create a subscriber to the gripper's state
    sub_state = rnh.subscribe("/robot/end_effector/" + _limb + "_gripper/state",
                                  SUBSCRIBER_BUFFER, &Gripper::gripperCb, this);

    //Initially all the interesting properties of the state are unknown
    EndEffectorState init_state;
    init_state.calibrated = EndEffectorState::STATE_UNKNOWN;
    init_state.enabled    = EndEffectorState::STATE_UNKNOWN;
    init_state.error      = EndEffectorState::STATE_UNKNOWN;
    init_state.gripping   = EndEffectorState::STATE_UNKNOWN;
    init_state.missed     = EndEffectorState::STATE_UNKNOWN;
    init_state.ready      = EndEffectorState::STATE_UNKNOWN;
    init_state.moving     = EndEffectorState::STATE_UNKNOWN;

    setGripperState(init_state);

    spinner.start();

    // sleep to wait for the publisher to be ready
    ros::Duration(0.5).sleep();

    // set the gripper parameters to their defaults
    setParameters("", true);
}

void Gripper::setGripperState(const baxter_core_msgs::EndEffectorState& _state)
{
    std::lock_guard<std::mutex> lock(mutex_state);
    state = _state;
}

baxter_core_msgs::EndEffectorState Gripper::getGripperState()
{
    std::lock_guard<std::mutex> lock(mutex_state);
    return state;
}

void Gripper::setGripperProperties(const baxter_core_msgs::EndEffectorProperties& _properties)
{
    std::lock_guard<std::mutex> lock(mutex_properties);
    properties = _properties;
}

baxter_core_msgs::EndEffectorProperties Gripper::getGripperProperties()
{
    std::lock_guard<std::mutex> lock(mutex_properties);
    return properties;
}

void Gripper::gripperCb(const EndEffectorState &msg)
{
    setGripperState(msg);

    if (first_run)
    {
        // wait for the properties to be set before issuing calibration command
        while(not prop_set)
        {
            ros::Duration(0.05).sleep();
        }

        if (!is_calibrated())
        {
            ROS_INFO("[%s_gripper] Calibrating the gripper..",
                                    getGripperLimb().c_str());
            calibrate();
        }

        first_run=false;
    }
}

void Gripper::gripperPropCb(const EndEffectorProperties &msg)
{
    setGripperProperties(msg);
    prop_set = true;

    // shut down the subscriber after the properties are set once
    // because these properties do not change
    sub_prop.shutdown();
}

void Gripper::setParameters(std::string _parameters, bool _defaults)
{
    if(_defaults)
    {
        parameters = validParameters();
    }
    else
    {
        if(_parameters != "")
        {
            // some error checking needed here to prevent invalid parameters being set
            // this would involve converting between strings and dict-type objects
            // and comparing to valid_parameters' keys
            parameters = _parameters;
        }
    }
    // send the parameters to the gripper
    std::string param_cmd = EndEffectorCommand::CMD_CONFIGURE;
    command(param_cmd, false, 0.0, parameters);
}

std::string Gripper::validParameters()
{
    std::string valid;

    if(type() == "electric")
    {
        valid = "{\"velocity\" : 50.0, \"moving_force\" : 40.0, \"holding_force\" : 30.0, \"dead_zone\" : 5.0}";
    }
    else if(type() == "suction")
    {
        valid = "{\"vacuum_sensor_threshold\" : 18.0, \"blow_off_seconds\" : 0.4}";
    }
    else
    {
        valid = "";
    }

    return valid;
}

void Gripper::calibrate(bool _block, double _timeout)
{
    if(type() != "electric") { capabilityWarning("calibrate"); }

    std::string calibrate_cmd = EndEffectorCommand::CMD_CALIBRATE;
    command(calibrate_cmd, _block, _timeout, "");
}

void Gripper::clearCalibration()
{
    if(type() != "electric") { capabilityWarning("clearCalibration"); }

    std::string clear_calibrate_cmd = EndEffectorCommand::CMD_CLEAR_CALIBRATION;
    command(clear_calibrate_cmd, false, 0.0, "");
}

bool Gripper::reboot()
{
    // give a warning if not capable and return
    if(type() != "electric")
    {
        capabilityWarning("reboot");
        return false;
    }

    ROS_INFO("Rebooting %s gripper of type %s. Please wait...",
             getGripperLimb().c_str(), type().c_str());

    std::string reboot_cmd = EndEffectorCommand::CMD_REBOOT;
    command(reboot_cmd, true, 5.0, "");

    ROS_INFO("Reboot complete");
    return true;
}

int Gripper::get_id()
{
    return getGripperState().id;
}

bool Gripper::is_enabled()
{
    return getGripperState().enabled==EndEffectorState::STATE_TRUE;
}

bool Gripper::is_calibrated()
{
    return getGripperState().calibrated==EndEffectorState::STATE_TRUE;
}

bool Gripper::is_ready_to_grip()
{
    return getGripperState().ready==EndEffectorState::STATE_TRUE;
}

bool Gripper::has_error()
{
    return getGripperState().error==EndEffectorState::STATE_TRUE;
}

bool Gripper::is_sucking()
{
    if(type() != "suction") { capabilityWarning("is_sucking"); }

    // ROS_INFO("force is: %g\n",getGripperState().force);
    return getGripperState().position<80;
}

bool Gripper::is_gripping()
{
    return getGripperState().gripping==EndEffectorState::STATE_TRUE;
}

bool Gripper::hasForce()
{
    return getGripperProperties().controls_force == true;
}

bool Gripper::hasPosition()
{
    return getGripperProperties().controls_position == true;
}

bool Gripper::open(bool _block, double _timeout)
{
    if(type() == "electric")
    {
        return commandPosition(100.0, _block, _timeout);
    }
    else if (type() == "suction")
    {
        // check if the gripper is already not sucking
        if(not is_sucking())
        {
            ROS_WARN("%s gripper of type %s is already open",
                      getGripperLimb().c_str(), type().c_str());

            return false;
        }

        return stop(_block, _timeout);
    }
    else
    {
        // give a warning if not capable and return
        capabilityWarning("open");
        return false;
    }
}

bool Gripper::close(bool _block, double _timeout)
{
    if(type() == "electric")
    {
        return commandPosition(0.0, _block, _timeout);
    }
    else if (type() == "suction")
    {
        // no checks here for is_sucking() so that the suction time may be extended as necessary
        return commandSuction(_block, _timeout);
    }
    else
    {
        // give a warning if not capable and return
        capabilityWarning("close");
        return false;
    }
}

bool Gripper::commandPosition(double _position, bool _block, double _timeout)
{
    // give a warning if not capable and return
    if(type() != "electric")
    {
        capabilityWarning("commandPosition");
        return false;
    }

    // calibrate the electric gripper if needed
    // do it in a blocking manner so that the gripper does not move before calibration
    if(not is_calibrated())
    {
        ROS_INFO("Calibrating gripper. Please wait...");
        calibrate(true, 2.0);
        ROS_INFO("Calibration complete");
    }

    // ensures that the gripper is positioned within physical limits
    if(_position >= 0.0 && _position <= 100.0)
    {
        std::string position_cmd = EndEffectorCommand::CMD_GO;
        std::string position_args = "{\"position\": " +
                                     std::to_string(_position) + "}";

        return command(position_cmd, _block, _timeout, position_args);
    }
    else
    {
        ROS_WARN("%s gripper of type %s must have position between 0.0 and 100.0",
                                        getGripperLimb().c_str(), type().c_str());
        return false;
    }
}

bool Gripper::commandSuction(bool _block, double _timeout)
{
    // give a warning if not capable and return
    if(type() != "suction")
    {
        capabilityWarning("commandSuction");
        return false;
    }

    std::string suction_cmd = EndEffectorCommand::CMD_GO;
    std::string suction_args = "{\"grip_attempt_seconds\": " +
                                std::to_string(_timeout) + "}";

    return command(suction_cmd, _block, _timeout, suction_args);
}

bool Gripper::stop(bool _block, double _timeout)
{
    std::string stop_cmd;

    if(type() == "electric")
    {
        stop_cmd = EndEffectorCommand::CMD_STOP;
    }
    else if(type() == "suction")
    {
        stop_cmd = EndEffectorCommand::CMD_RELEASE;
    }
    else
    {
        // give a warning if not capable and return
        capabilityWarning("stop");
        return false;
    }

    return command(stop_cmd, _block, _timeout);
}

bool Gripper::command(std::string _cmd, bool _block,
                      double _timeout, std::string _args)
{
    EndEffectorCommand ee_cmd;
    ee_cmd.id = get_id();
    ee_cmd.command = _cmd;
    ee_cmd.sender = cmd_sender;
    ee_cmd.sequence = incCmdSeq();
    ee_cmd.args = "";
    if(_args != "")
    {
        ee_cmd.args = _args;
    }

    pub_cmd.publish(ee_cmd);

    if(_block)
    {
        ros::Duration timeout(_timeout);
        wait(timeout);
    }

    return true;
}

void Gripper::capabilityWarning(std::string _function)
{
    ROS_WARN("%s gripper of type %s is not capable of %s",
              getGripperLimb().c_str(), type().c_str(), _function.c_str());
}

int Gripper::incCmdSeq()
{
    // prevents cmd_sequence from overflowing the integer limit
    cmd_sequence = (cmd_sequence % std::numeric_limits<int>::max()) + 1;
    return cmd_sequence;
}

std::string Gripper::type()
{
    int ui_code = (int) getGripperProperties().ui_type;

    if (ui_code == 1)
    {
        return "suction";
    }
    else if (ui_code == 2)
    {
        return "electric";
    }
    else
    {
        return "uninitialized";
    }
}

bool Gripper::wait(ros::Duration _timeout)
{
    // waits until the difference between the start and
    // current time catches up to the timeout
    ros::Rate r(100);
    ros::Time start = ros::Time::now();

    while(ros::ok())
    {
        if (ros::Time::now() - start > _timeout) { return true; };

        r.sleep();
    }

    return false;
}

Gripper::~Gripper()
{
    spinner.stop();
}
<commit_msg>[Gripper] change to calibration wait time and debug messages<commit_after>#include "robot_interface/gripper.h"

#include <iostream>
#include <string>

using namespace baxter_core_msgs;
using namespace std;

Gripper::Gripper(std::string _limb, bool _use_robot) :
                 rnh(_limb), limb(_limb), use_robot(_use_robot), first_run(true), prop_set(false),
                 spinner(1), cmd_sequence(0), cmd_sender(ros::this_node::getName())
{
    if (not use_robot) return;

    // create a publisher for the gripper's commands
    pub_cmd = rnh.advertise<EndEffectorCommand>( "/robot/end_effector/" +
                                                _limb + "_gripper/command", 1);

    // create a subscriber to the gripper's properties
    sub_prop  = rnh.subscribe("/robot/end_effector/" + _limb + "_gripper/properties",
                                   SUBSCRIBER_BUFFER, &Gripper::gripperPropCb, this);

    // create a subscriber to the gripper's state
    sub_state = rnh.subscribe("/robot/end_effector/" + _limb + "_gripper/state",
                                  SUBSCRIBER_BUFFER, &Gripper::gripperCb, this);

    //Initially all the interesting properties of the state are unknown
    EndEffectorState init_state;
    init_state.calibrated = EndEffectorState::STATE_UNKNOWN;
    init_state.enabled    = EndEffectorState::STATE_UNKNOWN;
    init_state.error      = EndEffectorState::STATE_UNKNOWN;
    init_state.gripping   = EndEffectorState::STATE_UNKNOWN;
    init_state.missed     = EndEffectorState::STATE_UNKNOWN;
    init_state.ready      = EndEffectorState::STATE_UNKNOWN;
    init_state.moving     = EndEffectorState::STATE_UNKNOWN;

    setGripperState(init_state);

    spinner.start();

    // sleep to wait for the publisher to be ready
    ros::Duration(0.5).sleep();

    // set the gripper parameters to their defaults
    setParameters("", true);
}

void Gripper::setGripperState(const baxter_core_msgs::EndEffectorState& _state)
{
    std::lock_guard<std::mutex> lock(mutex_state);
    state = _state;
}

baxter_core_msgs::EndEffectorState Gripper::getGripperState()
{
    std::lock_guard<std::mutex> lock(mutex_state);
    return state;
}

void Gripper::setGripperProperties(const baxter_core_msgs::EndEffectorProperties& _properties)
{
    std::lock_guard<std::mutex> lock(mutex_properties);
    properties = _properties;
}

baxter_core_msgs::EndEffectorProperties Gripper::getGripperProperties()
{
    std::lock_guard<std::mutex> lock(mutex_properties);
    return properties;
}

void Gripper::gripperCb(const EndEffectorState &msg)
{
    setGripperState(msg);

    if (first_run)
    {
        // wait for the properties to be set before issuing calibration command
        while(not prop_set)
        {
            ros::Duration(0.05).sleep();
        }

        if (!is_calibrated())
        {
            ROS_INFO("[%s_gripper] Calibrating the gripper..",
                                    getGripperLimb().c_str());
            calibrate();
        }

        first_run=false;
    }
}

void Gripper::gripperPropCb(const EndEffectorProperties &msg)
{
    setGripperProperties(msg);
    prop_set = true;

    // shut down the subscriber after the properties are set once
    // because these properties do not change
    sub_prop.shutdown();
}

void Gripper::setParameters(std::string _parameters, bool _defaults)
{
    if(_defaults)
    {
        parameters = validParameters();
    }
    else
    {
        if(_parameters != "")
        {
            // some error checking needed here to prevent invalid parameters being set
            // this would involve converting between strings and dict-type objects
            // and comparing to valid_parameters' keys
            parameters = _parameters;
        }
    }
    // send the parameters to the gripper
    std::string param_cmd = EndEffectorCommand::CMD_CONFIGURE;
    command(param_cmd, false, 0.0, parameters);
}

std::string Gripper::validParameters()
{
    std::string valid;

    if(type() == "electric")
    {
        valid = "{\"velocity\" : 50.0, \"moving_force\" : 40.0, \"holding_force\" : 30.0, \"dead_zone\" : 5.0}";
    }
    else if(type() == "suction")
    {
        valid = "{\"vacuum_sensor_threshold\" : 18.0, \"blow_off_seconds\" : 0.4}";
    }
    else
    {
        valid = "";
    }

    return valid;
}

void Gripper::calibrate(bool _block, double _timeout)
{
    if(type() != "electric") { capabilityWarning("calibrate"); }

    std::string calibrate_cmd = EndEffectorCommand::CMD_CALIBRATE;
    command(calibrate_cmd, _block, _timeout, "");
}

void Gripper::clearCalibration()
{
    if(type() != "electric") { capabilityWarning("clearCalibration"); }

    std::string clear_calibrate_cmd = EndEffectorCommand::CMD_CLEAR_CALIBRATION;
    command(clear_calibrate_cmd, false, 0.0, "");
}

bool Gripper::reboot()
{
    // give a warning if not capable and return
    if(type() != "electric")
    {
        capabilityWarning("reboot");
        return false;
    }

    ROS_INFO("Rebooting %s gripper of type %s. Please wait...",
             getGripperLimb().c_str(), type().c_str());

    std::string reboot_cmd = EndEffectorCommand::CMD_REBOOT;
    command(reboot_cmd, true, 5.0, "");

    ROS_INFO("Reboot complete");
    return true;
}

int Gripper::get_id()
{
    return getGripperState().id;
}

bool Gripper::is_enabled()
{
    return getGripperState().enabled==EndEffectorState::STATE_TRUE;
}

bool Gripper::is_calibrated()
{
    return getGripperState().calibrated==EndEffectorState::STATE_TRUE;
}

bool Gripper::is_ready_to_grip()
{
    return getGripperState().ready==EndEffectorState::STATE_TRUE;
}

bool Gripper::has_error()
{
    return getGripperState().error==EndEffectorState::STATE_TRUE;
}

bool Gripper::is_sucking()
{
    if(type() != "suction") { capabilityWarning("is_sucking"); }

    // ROS_INFO("force is: %g\n",getGripperState().force);
    return getGripperState().position<80;
}

bool Gripper::is_gripping()
{
    return getGripperState().gripping==EndEffectorState::STATE_TRUE;
}

bool Gripper::hasForce()
{
    return getGripperProperties().controls_force == true;
}

bool Gripper::hasPosition()
{
    return getGripperProperties().controls_position == true;
}

bool Gripper::open(bool _block, double _timeout)
{
    if(type() == "electric")
    {
        return commandPosition(100.0, _block, _timeout);
    }
    else if (type() == "suction")
    {
        // check if the gripper is already not sucking
        if(not is_sucking())
        {
            ROS_WARN("%s gripper of type %s is already open",
                      getGripperLimb().c_str(), type().c_str());

            return false;
        }

        return stop(_block, _timeout);
    }
    else
    {
        // give a warning if not capable and return
        capabilityWarning("open");
        return false;
    }
}

bool Gripper::close(bool _block, double _timeout)
{
    if(type() == "electric")
    {
        return commandPosition(0.0, _block, _timeout);
    }
    else if (type() == "suction")
    {
        // no checks here for is_sucking() so that the suction time may be extended as necessary
        return commandSuction(_block, _timeout);
    }
    else
    {
        // give a warning if not capable and return
        capabilityWarning("close");
        return false;
    }
}

bool Gripper::commandPosition(double _position, bool _block, double _timeout)
{
    // give a warning if not capable and return
    if(type() != "electric")
    {
        capabilityWarning("commandPosition");
        return false;
    }

    // calibrate the electric gripper if needed
    // do it in a blocking manner so that the gripper does not move before calibration
    if(not is_calibrated())
    {
        ROS_INFO("Calibrating gripper. Please wait...");
        calibrate(true, 3.0);
        ROS_INFO("Calibration complete");
    }

    // ensures that the gripper is positioned within physical limits
    if(_position >= 0.0 && _position <= 100.0)
    {
        ROS_DEBUG("Commanding position %g", _position);
        std::string position_cmd = EndEffectorCommand::CMD_GO;
        std::string position_args = "{\"position\": " +
                                     std::to_string(_position) + "}";

        return command(position_cmd, _block, _timeout, position_args);
    }
    else
    {
        ROS_WARN("%s gripper of type %s must have position between 0.0 and 100.0",
                                        getGripperLimb().c_str(), type().c_str());
        return false;
    }
}

bool Gripper::commandSuction(bool _block, double _timeout)
{
    // give a warning if not capable and return
    if(type() != "suction")
    {
        capabilityWarning("commandSuction");
        return false;
    }

    std::string suction_cmd = EndEffectorCommand::CMD_GO;
    std::string suction_args = "{\"grip_attempt_seconds\": " +
                                std::to_string(_timeout) + "}";

    return command(suction_cmd, _block, _timeout, suction_args);
}

bool Gripper::stop(bool _block, double _timeout)
{
    std::string stop_cmd;

    if(type() == "electric")
    {
        stop_cmd = EndEffectorCommand::CMD_STOP;
    }
    else if(type() == "suction")
    {
        stop_cmd = EndEffectorCommand::CMD_RELEASE;
    }
    else
    {
        // give a warning if not capable and return
        capabilityWarning("stop");
        return false;
    }

    return command(stop_cmd, _block, _timeout);
}

bool Gripper::command(std::string _cmd, bool _block,
                      double _timeout, std::string _args)
{
    EndEffectorCommand ee_cmd;
    ee_cmd.id = get_id();
    ee_cmd.command = _cmd;
    ee_cmd.sender = cmd_sender;
    ee_cmd.sequence = incCmdSeq();
    ee_cmd.args = "";
    if(_args != "")
    {
        ee_cmd.args = _args;
    }

    pub_cmd.publish(ee_cmd);

    if(_block)
    {
        ros::Duration timeout(_timeout);
        return wait(timeout);
    }

    return true;
}

void Gripper::capabilityWarning(std::string _function)
{
    ROS_WARN("%s gripper of type %s is not capable of %s",
              getGripperLimb().c_str(), type().c_str(), _function.c_str());
}

int Gripper::incCmdSeq()
{
    // prevents cmd_sequence from overflowing the integer limit
    cmd_sequence = (cmd_sequence % std::numeric_limits<int>::max()) + 1;
    return cmd_sequence;
}

std::string Gripper::type()
{
    int ui_code = (int) getGripperProperties().ui_type;

    if (ui_code == 1)
    {
        return "suction";
    }
    else if (ui_code == 2)
    {
        return "electric";
    }
    else
    {
        return "uninitialized";
    }
}

bool Gripper::wait(ros::Duration _timeout)
{
    // waits until the difference between the start and
    // current time catches up to the timeout
    ros::Rate r(100);
    ros::Time start = ros::Time::now();

    while(ros::ok())
    {
        ROS_DEBUG("Waiting...");
        if (ros::Time::now() - start > _timeout) { return true; };

        r.sleep();
    }

    return false;
}

Gripper::~Gripper()
{
    spinner.stop();
}
<|endoftext|>


<commit_before>/*
 *
 * Copyright 2016 gRPC authors.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "test/cpp/qps/parse_json.h"

#include <string>

#include <grpc/support/log.h>

namespace grpc {
namespace testing {

void ParseJson(const std::string& json, const std::string& type,
               GRPC_CUSTOM_MESSAGE* msg) {
  std::unique_ptr<protobuf::json::TypeResolver> type_resolver(
      protobuf::json::NewTypeResolverForDescriptorPool(
          "type.googleapis.com", protobuf::DescriptorPool::generated_pool()));
  std::string binary;
  auto status = JsonToBinaryString(
      type_resolver.get(), "type.googleapis.com/" + type, json, &binary);
  if (!status.ok()) {
    std::string errmsg(status.error_message());
    gpr_log(GPR_ERROR, "Failed to convert json to binary: errcode=%d msg=%s",
            status.code(), errmsg.c_str());
    gpr_log(GPR_ERROR, "JSON: %s", json.c_str());
    abort();
  }
  GPR_ASSERT(msg->ParseFromString(binary));
}

std::string SerializeJson(const GRPC_CUSTOM_MESSAGE& msg,
                          const std::string& type) {
  std::unique_ptr<protobuf::json::TypeResolver> type_resolver(
      protobuf::json::NewTypeResolverForDescriptorPool(
          "type.googleapis.com", protobuf::DescriptorPool::generated_pool()));
  std::string binary;
  std::string json_string;
  msg.SerializeToString(&binary);
  auto status =
      BinaryToJsonString(type_resolver.get(), type, binary, &json_string);
  GPR_ASSERT(status.ok());
  return json_string;
}

}  // namespace testing
}  // namespace grpc
<commit_msg>Fix status error (#25731)<commit_after>/*
 *
 * Copyright 2016 gRPC authors.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "test/cpp/qps/parse_json.h"

#include <string>

#include <grpc/support/log.h>

namespace grpc {
namespace testing {

void ParseJson(const std::string& json, const std::string& type,
               GRPC_CUSTOM_MESSAGE* msg) {
  std::unique_ptr<protobuf::json::TypeResolver> type_resolver(
      protobuf::json::NewTypeResolverForDescriptorPool(
          "type.googleapis.com", protobuf::DescriptorPool::generated_pool()));
  std::string binary;
  auto status = JsonToBinaryString(
      type_resolver.get(), "type.googleapis.com/" + type, json, &binary);
  if (!status.ok()) {
    std::string errmsg(status.message());
    gpr_log(GPR_ERROR, "Failed to convert json to binary: errcode=%d msg=%s",
            status.code(), errmsg.c_str());
    gpr_log(GPR_ERROR, "JSON: %s", json.c_str());
    abort();
  }
  GPR_ASSERT(msg->ParseFromString(binary));
}

std::string SerializeJson(const GRPC_CUSTOM_MESSAGE& msg,
                          const std::string& type) {
  std::unique_ptr<protobuf::json::TypeResolver> type_resolver(
      protobuf::json::NewTypeResolverForDescriptorPool(
          "type.googleapis.com", protobuf::DescriptorPool::generated_pool()));
  std::string binary;
  std::string json_string;
  msg.SerializeToString(&binary);
  auto status =
      BinaryToJsonString(type_resolver.get(), type, binary, &json_string);
  GPR_ASSERT(status.ok());
  return json_string;
}

}  // namespace testing
}  // namespace grpc
<|endoftext|>


<commit_before>/* Copyright 2015 Samsung Electronics Co., LTD
 *
 * 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.
 */

/***************************************************************************
 * Holds scene objects. Can be used by engines.
 ***************************************************************************/

#include "engine/renderer/renderer.h"
#include "objects/lightlist.h"
#include "objects/light.h"
#include "objects/scene.h"
#include "shaders/shader.h"

#define LIGHT_ADDED 1
#define LIGHT_REMOVED 2
#define SHADOW_CHANGED 4
#define REBUILD_SHADERS 8

namespace gvr {

LightList::~LightList()
{
    clear();
}

int LightList::getLights(std::vector<Light*>& lightList) const
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    for (auto itc = mClassMap.begin(); itc != mClassMap.end(); ++itc)
    {
        const std::vector<Light*>& lights = itc->second;
        for (auto itl = lights.begin(); itl != lights.end(); ++itl)
        {
            Light* light = *itl;
            lightList.push_back(light);
        }
    }
    return lightList.size();
}

/*
 * Adds a new light to the scene.
 * Return true if light was added, false if already there or too many lights.
 */
bool LightList::addLight(Light* light)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);

    auto it2 = mClassMap.find(light->getLightClass());
    if (it2 != mClassMap.end())
    {
        std::vector<Light*>& lights = it2->second;
        light->setLightIndex(lights.size());
        lights.push_back(light);
    }
    else
    {
        std::pair<std::string, std::vector<Light*>> pair;

        light->setLightIndex(0);
        pair.first = light->getLightClass();
        pair.second.push_back(light);
        mClassMap.insert(pair);
    }
    mDirty |= LIGHT_ADDED | REBUILD_SHADERS;
#ifdef DEBUG_LIGHT
    LOGD("LIGHT: %s added to scene", light->getLightClass());
#endif
    return true;
}

/*
 * Removes an existing light from the scene.
 * Return true if light was removed, false if light was not in the scene.
 */
bool LightList::removeLight(Light* light)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);

    /*
     * Find the list of lights of this type in the light class map.
     */
    auto it3 = mClassMap.find(light->getLightClass());
    if (it3 != mClassMap.end())
    {
        std::vector<Light*>& lights = it3->second;
        auto it2 = std::find(lights.begin(), lights.end(), light);
        if (it2 == lights.end())
        {
            return false;
        }
        light->setLightIndex(-1);
        /*
         * If all lights in the class are gone,
         * remove the class from the map.
         */
        if (lights.size() == 1)
        {
            mClassMap.erase(it3);
            mDirty |= LIGHT_REMOVED | REBUILD_SHADERS;
            return true;
        }
        /*
         * Removed a light, recompute light indices for all
         * lights of that type
         */
        else
        {
            int index = 0;
            lights.erase(it2);
            for (auto it = lights.begin();
                 it != lights.end();
                 ++it)
            {
                Light* l = *it;
                l->setLightIndex(index++);
            }
        }
    }
#ifdef DEBUG_LIGHT
    LOGD("LIGHT: %s removed from scene", light->getLightClass());
#endif
    mDirty |= LIGHT_REMOVED | REBUILD_SHADERS;
    return true;
}

void LightList::shadersRebuilt()
{
    mDirty &= ~REBUILD_SHADERS;
}

void LightList::forEachLight(std::function<void(Light&)> func)
{
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;
            if (light)
            {
                func(*light);
            }
        }
    }
}

void LightList::forEachLight(std::function<void(const Light&)> func) const
{
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;
            if (light)
            {
                func(*light);
            }
        }
    }
}

ShadowMap* LightList::scanLights()
{
    ShadowMap* shadowMap = NULL;

    mTotalUniforms = 0;
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;

            mTotalUniforms += light->uniforms().uniforms().getNumEntries();
            ShadowMap* sm = light->getShadowMap();
            if (sm && sm->enabled())
            {
                shadowMap = sm;
            }
        }
    }
    return shadowMap;
}

ShadowMap* LightList::updateLightBlock(Renderer* renderer)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    bool dirty = mDirty != 0;
    ShadowMap* shadowMap = NULL;

    if (mDirty & REBUILD_SHADERS)
    {
        return scanLights();
    }
    if (mDirty & LIGHT_ADDED)
    {
        createLightBlock(renderer);
    }
    mTotalUniforms = 0;
    if (mClassMap.size() == 0)
    {
        if (mLightBlock != nullptr)
        {
            delete mLightBlock;
            mLightBlock = nullptr;
#ifdef DEBUG_LIGHT
            LOGD("LIGHT: clearing light uniform block");
#endif
        }
        return NULL;
    }
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;
            ShadowMap* sm = light->getShadowMap();

            mTotalUniforms += light->uniforms().uniforms().getNumEntries();
            if (sm && sm->enabled())
            {
                shadowMap = sm;
            }
            if (dirty || light->uniforms().isDirty(ShaderData::MAT_DATA))
            {
                int offset = light->getBlockOffset();
                const UniformBlock& uniforms = light->uniforms().uniforms();
                mLightBlock->updateGPU(renderer, offset, uniforms);
                light->uniforms().clearDirty();
            }
        }
    }
    mDirty = 0;
    return shadowMap;
}

void LightList::useLights(Renderer* renderer, Shader* shader)
{
    if (mUseUniformBlock)
    {
        if (mLightBlock)
        {
            mLightBlock->bindBuffer(shader, renderer);
        }
    }
    else
    {
        shader->bindLights(*this, renderer);
    }
}

void LightList::makeShadowMaps(Scene* scene, jobject jscene, ShaderManager* shaderManager)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    int layerIndex = 0;
    int numShadowMaps = 0;

    for (auto it2 = mClassMap.begin(); it2 != mClassMap.end(); ++it2)
    {
        const std::vector<Light*>& lights = it2->second;
        for (auto it = lights.begin(); it != lights.end(); ++it)
        {
            Light *l = (*it);
            if (l->enabled())
            {
                if (l->makeShadowMap(scene, jscene, shaderManager, layerIndex))
                {
                    ++numShadowMaps;
                    ++layerIndex;
                }
            }
        }
    }
    if (mNumShadowMaps != numShadowMaps)
    {
        mNumShadowMaps = numShadowMaps;
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: %d shadow maps", mNumShadowMaps);
#endif
    }
}

bool LightList::createLightBlock(Renderer* renderer)
{
    int numFloats = 0;

    for (auto it = mClassMap.begin(); it != mClassMap.end(); ++it)
    {
        const std::vector<Light*>& lights = it->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light *light = (*it2);
            light->setBlockOffset(numFloats);
            numFloats += light->getTotalSize() / sizeof(float);
        }
    }
    if (mLightBlock && (numFloats > (mLightBlock->getTotalSize() / sizeof(float))))
    {
        delete mLightBlock;
        mLightBlock = nullptr;
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: deleting light uniform block");
#endif
    }
    if (mLightBlock == nullptr)
    {
        mLightBlock = renderer->createUniformBlock("float lightdata", LIGHT_UBO_INDEX,
                                                   "Lights_ubo", numFloats);
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: creating light uniform block %d floats", numFloats);
#endif
        mLightBlock->useGPUBuffer(true);
    }
    return true;
}

/*
 * Removes all the lights from the scene.
 * This function should only be called from the GL thread.
 */
void LightList::clear()
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    mClassMap.clear();
    mDirty = LIGHT_REMOVED | REBUILD_SHADERS;
    if (mLightBlock)
    {
        delete mLightBlock;
        mLightBlock = nullptr;
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: clearing light uniform block");
#endif
    }
#ifdef DEBUG_LIGHT
    LOGD("LIGHT: clearing lights");
#endif
}

void LightList::makeShaderBlock(std::string& layout) const
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    std::ostringstream stream;
    std::string prefix;

    if (mUseUniformBlock)
    {
        int uboLightBinding = LIGHT_UBO_INDEX;
        stream <<
                "\n#ifdef VULKAN\n"
                "layout (std140, set = 0, binding = " << uboLightBinding << ")"
                "\n#else\n"
                "layout (std140)"
                "\n#endif\n"
                "uniform Lights_ubo\n{"
               << std::endl;
    }
    else
    {
        prefix = "uniform ";
    }
    for (auto it = mClassMap.begin(); it != mClassMap.end(); ++it)
    {
        const std::vector<Light*>& lights = it->second;
        stream << prefix << 'U' << it->first << " " << it->first << "s[" << lights.size() << "];" << std::endl;
    }
    if (mUseUniformBlock)
    {
        stream << "};" << std::endl;
    }
    layout = stream.str();
}

}
<commit_msg>Dirty flag of lights not cleared at all<commit_after>/* Copyright 2015 Samsung Electronics Co., LTD
 *
 * 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.
 */

/***************************************************************************
 * Holds scene objects. Can be used by engines.
 ***************************************************************************/

#include "engine/renderer/renderer.h"
#include "objects/lightlist.h"
#include "objects/light.h"
#include "objects/scene.h"
#include "shaders/shader.h"

#define LIGHT_ADDED 1
#define LIGHT_REMOVED 2
#define SHADOW_CHANGED 4
#define REBUILD_SHADERS 8

namespace gvr {

LightList::~LightList()
{
    clear();
}

int LightList::getLights(std::vector<Light*>& lightList) const
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    for (auto itc = mClassMap.begin(); itc != mClassMap.end(); ++itc)
    {
        const std::vector<Light*>& lights = itc->second;
        for (auto itl = lights.begin(); itl != lights.end(); ++itl)
        {
            Light* light = *itl;
            lightList.push_back(light);
        }
    }
    return lightList.size();
}

/*
 * Adds a new light to the scene.
 * Return true if light was added, false if already there or too many lights.
 */
bool LightList::addLight(Light* light)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);

    auto it2 = mClassMap.find(light->getLightClass());
    if (it2 != mClassMap.end())
    {
        std::vector<Light*>& lights = it2->second;
        light->setLightIndex(lights.size());
        lights.push_back(light);
    }
    else
    {
        std::pair<std::string, std::vector<Light*>> pair;

        light->setLightIndex(0);
        pair.first = light->getLightClass();
        pair.second.push_back(light);
        mClassMap.insert(pair);
    }
    mDirty |= LIGHT_ADDED | REBUILD_SHADERS;
#ifdef DEBUG_LIGHT
    LOGD("LIGHT: %s added to scene", light->getLightClass());
#endif
    return true;
}

/*
 * Removes an existing light from the scene.
 * Return true if light was removed, false if light was not in the scene.
 */
bool LightList::removeLight(Light* light)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);

    /*
     * Find the list of lights of this type in the light class map.
     */
    auto it3 = mClassMap.find(light->getLightClass());
    if (it3 != mClassMap.end())
    {
        std::vector<Light*>& lights = it3->second;
        auto it2 = std::find(lights.begin(), lights.end(), light);
        if (it2 == lights.end())
        {
            return false;
        }
        light->setLightIndex(-1);
        /*
         * If all lights in the class are gone,
         * remove the class from the map.
         */
        if (lights.size() == 1)
        {
            mClassMap.erase(it3);
            mDirty |= LIGHT_REMOVED | REBUILD_SHADERS;
            return true;
        }
        /*
         * Removed a light, recompute light indices for all
         * lights of that type
         */
        else
        {
            int index = 0;
            lights.erase(it2);
            for (auto it = lights.begin();
                 it != lights.end();
                 ++it)
            {
                Light* l = *it;
                l->setLightIndex(index++);
            }
        }
    }
#ifdef DEBUG_LIGHT
    LOGD("LIGHT: %s removed from scene", light->getLightClass());
#endif
    mDirty |= LIGHT_REMOVED | REBUILD_SHADERS;
    return true;
}

void LightList::shadersRebuilt()
{
    mDirty &= ~REBUILD_SHADERS;
}

void LightList::forEachLight(std::function<void(Light&)> func)
{
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;
            if (light)
            {
                func(*light);
            }
        }
    }
}

void LightList::forEachLight(std::function<void(const Light&)> func) const
{
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;
            if (light)
            {
                func(*light);
            }
        }
    }
}

ShadowMap* LightList::scanLights()
{
    ShadowMap* shadowMap = NULL;

    mTotalUniforms = 0;
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;

            mTotalUniforms += light->uniforms().uniforms().getNumEntries();
            ShadowMap* sm = light->getShadowMap();
            if (sm && sm->enabled())
            {
                shadowMap = sm;
            }
        }
    }
    return shadowMap;
}

ShadowMap* LightList::updateLightBlock(Renderer* renderer)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    bool dirty = mDirty != 0;
    ShadowMap* shadowMap = NULL;

    if (mDirty & REBUILD_SHADERS)
    {
        return scanLights();
    }
    if (mDirty & LIGHT_ADDED)
    {
        createLightBlock(renderer);
    }
    mTotalUniforms = 0;
    if (mClassMap.size() == 0)
    {
        if (mLightBlock != nullptr)
        {
            delete mLightBlock;
            mLightBlock = nullptr;
#ifdef DEBUG_LIGHT
            LOGD("LIGHT: clearing light uniform block");
#endif
        }
        mDirty = 0;
        return NULL;
    }
    for (auto it1 = mClassMap.begin();
         it1 != mClassMap.end();
         ++it1)
    {
        const std::vector<Light*>& lights = it1->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light* light = *it2;
            ShadowMap* sm = light->getShadowMap();

            mTotalUniforms += light->uniforms().uniforms().getNumEntries();
            if (sm && sm->enabled())
            {
                shadowMap = sm;
            }
            if (dirty || light->uniforms().isDirty(ShaderData::MAT_DATA))
            {
                int offset = light->getBlockOffset();
                const UniformBlock& uniforms = light->uniforms().uniforms();
                mLightBlock->updateGPU(renderer, offset, uniforms);
                light->uniforms().clearDirty();
            }
        }
    }
    mDirty = 0;
    return shadowMap;
}

void LightList::useLights(Renderer* renderer, Shader* shader)
{
    if (mUseUniformBlock)
    {
        if (mLightBlock)
        {
            mLightBlock->bindBuffer(shader, renderer);
        }
    }
    else
    {
        shader->bindLights(*this, renderer);
    }
}

void LightList::makeShadowMaps(Scene* scene, jobject jscene, ShaderManager* shaderManager)
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    int layerIndex = 0;
    int numShadowMaps = 0;

    for (auto it2 = mClassMap.begin(); it2 != mClassMap.end(); ++it2)
    {
        const std::vector<Light*>& lights = it2->second;
        for (auto it = lights.begin(); it != lights.end(); ++it)
        {
            Light *l = (*it);
            if (l->enabled())
            {
                if (l->makeShadowMap(scene, jscene, shaderManager, layerIndex))
                {
                    ++numShadowMaps;
                    ++layerIndex;
                }
            }
        }
    }
    if (mNumShadowMaps != numShadowMaps)
    {
        mNumShadowMaps = numShadowMaps;
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: %d shadow maps", mNumShadowMaps);
#endif
    }
}

bool LightList::createLightBlock(Renderer* renderer)
{
    int numFloats = 0;

    for (auto it = mClassMap.begin(); it != mClassMap.end(); ++it)
    {
        const std::vector<Light*>& lights = it->second;
        for (auto it2 = lights.begin(); it2 != lights.end(); ++it2)
        {
            Light *light = (*it2);
            light->setBlockOffset(numFloats);
            numFloats += light->getTotalSize() / sizeof(float);
        }
    }
    if (mLightBlock && (numFloats > (mLightBlock->getTotalSize() / sizeof(float))))
    {
        delete mLightBlock;
        mLightBlock = nullptr;
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: deleting light uniform block");
#endif
    }
    if (mLightBlock == nullptr)
    {
        mLightBlock = renderer->createUniformBlock("float lightdata", LIGHT_UBO_INDEX,
                                                   "Lights_ubo", numFloats);
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: creating light uniform block %d floats", numFloats);
#endif
        mLightBlock->useGPUBuffer(true);
    }
    return true;
}

/*
 * Removes all the lights from the scene.
 * This function should only be called from the GL thread.
 */
void LightList::clear()
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    mClassMap.clear();
    mDirty = LIGHT_REMOVED | REBUILD_SHADERS;
    if (mLightBlock)
    {
        delete mLightBlock;
        mLightBlock = nullptr;
#ifdef DEBUG_LIGHT
        LOGD("LIGHT: clearing light uniform block");
#endif
    }
#ifdef DEBUG_LIGHT
    LOGD("LIGHT: clearing lights");
#endif
}

void LightList::makeShaderBlock(std::string& layout) const
{
    std::lock_guard < std::recursive_mutex > lock(mLock);
    std::ostringstream stream;
    std::string prefix;

    if (mUseUniformBlock)
    {
        int uboLightBinding = LIGHT_UBO_INDEX;
        stream <<
                "\n#ifdef VULKAN\n"
                "layout (std140, set = 0, binding = " << uboLightBinding << ")"
                "\n#else\n"
                "layout (std140)"
                "\n#endif\n"
                "uniform Lights_ubo\n{"
               << std::endl;
    }
    else
    {
        prefix = "uniform ";
    }
    for (auto it = mClassMap.begin(); it != mClassMap.end(); ++it)
    {
        const std::vector<Light*>& lights = it->second;
        stream << prefix << 'U' << it->first << " " << it->first << "s[" << lights.size() << "];" << std::endl;
    }
    if (mUseUniformBlock)
    {
        stream << "};" << std::endl;
    }
    layout = stream.str();
}

}
<|endoftext|>


<commit_before>#include "eventbackend.h"

#define MAX_EVENTS 1024

#if defined(__linux__)

// epoll needs kernel 2.5.44

// Linux
#include <sys/epoll.h>

// POSIX
#include <fcntl.h>

// POSIX++
#include <cstring> // for strerror()
#include <cstdlib> // for exit

// PDTK
#include <cxxutils/vterm.h>

struct EventBackend::platform_dependant // poll notification (epoll)
{
  posix::fd_t fd;
  struct epoll_event output[MAX_EVENTS];

  platform_dependant(void) noexcept
    : fd(posix::invalid_descriptor)
  {
    fd = ::epoll_create(MAX_EVENTS);
    flaw(fd == posix::invalid_descriptor, terminal::critical, std::exit(errno),,
         "Unable to create an instance of epoll! %s", std::strerror(errno))
  }

  ~platform_dependant(void) noexcept
  {
    posix::close(fd);
    fd = posix::invalid_descriptor;
  }

  bool add(posix::fd_t wd, native_flags_t flags) noexcept
  {
    struct epoll_event native_event;
    native_event.data.fd = wd;
    native_event.events = flags; // be sure to convert to native events
    return ::epoll_ctl(fd, EPOLL_CTL_ADD, wd, &native_event) == posix::success_response;
  }

  bool modify(posix::fd_t wd, native_flags_t flags) noexcept
  {
    struct epoll_event native_event;
    native_event.data.fd = wd;
    native_event.events = flags; // be sure to convert to native events

    return ::epoll_ctl(fd, EPOLL_CTL_MOD, wd, &native_event) == posix::success_response; // try to modify FD first
  }

  bool remove(posix::fd_t wd) noexcept
  {
    struct epoll_event event;
    return ::epoll_ctl(fd, EPOLL_CTL_DEL, wd, &event) == posix::success_response; // try to delete entry
  }
} EventBackend::s_platform;

lockable<std::unordered_multimap<posix::fd_t, EventBackend::callback_info_t>> EventBackend::queue;
std::list<std::pair<posix::fd_t, native_flags_t>> EventBackend::results;

bool EventBackend::add(posix::fd_t fd, native_flags_t flags, callback_t function) noexcept
{
  if(s_platform.add(fd, flags))
    queue.emplace(fd, (callback_info_t){flags, function});
  else if(errno == EEXIST && s_platform.modify(fd, flags)) // if entry exist and modification worked
  {
    bool found = false;
    auto entries = queue.equal_range(fd); // find modified entry!
    for(auto& pos = entries.first; pos != entries.second; ++pos)
    {
      found |= &(pos->second.function) == &function; // save if function was ever found
      if(&(pos->second.function) == &function) // if the FD is in the queue and it's callback function matches
        pos->second.flags |= flags; // simply modify the flags to include the current
    }

    if(!found) // function wasn't found
      queue.emplace(fd, (callback_info_t){flags, function}); // make a new entry for this FD
  }
  else // unable to add an entry or modify the entry for this FD!
    return false; // fail

  return true;
}

bool EventBackend::remove(posix::fd_t fd, native_flags_t flags) noexcept
{
  auto entries = queue.equal_range(fd); // find modified entry!
  for(auto& pos = entries.first; pos != entries.second; ++pos)
  {
    if(pos->second.flags == flags)
      queue.erase(pos);
    else if(pos->second.flags & flags)
    {
      pos->second.flags ^= pos->second.flags & flags;
      if(pos->second.flags) // if some flags are still set
        s_platform.modify(fd, pos->second.flags);
      else // if no flags are set then remove it completely
        s_platform.remove(fd);
    }
  }
  return true;
}

bool EventBackend::poll(int timeout) noexcept
{
  int count = ::epoll_wait(s_platform.fd, s_platform.output, MAX_EVENTS, timeout); // wait for new results
  results.clear(); // clear old results

  if(count == posix::error_response) // if error/timeout occurred
    return false; //fail

  const epoll_event* end = s_platform.output + count;
  for(epoll_event* pos = s_platform.output; pos != end; ++pos) // iterate through results
    results.emplace_back(std::make_pair(posix::fd_t(pos->data.fd), native_flags_t(pos->events))); // save result (in native format)
  return true;
}

namespace EventBackend
{
  static posix::fd_t s_pipeio[2] = { posix::invalid_descriptor }; //  execution stepper pipe
  static vfunc stepper_function = nullptr;
  enum {
    Read = 0,
    Write = 1,
  };

  void readstep(posix::fd_t fd, native_flags_t) noexcept
  {
    uint64_t discard;
    while(posix::read(fd, &discard, sizeof(discard)) != posix::error_response);
    stepper_function();
  }

  bool setstepper(vfunc function) noexcept
  {
    stepper_function = function;
    if(s_pipeio[Read] == posix::invalid_descriptor) // if execution stepper pipe  hasn't been initialized yet
    {
      flaw(::pipe(s_pipeio) == posix::error_response, terminal::critical, std::exit(errno), false,
           "Unable to create pipe for execution stepper: %s", std::strerror(errno))
      ::fcntl(s_pipeio[Read], F_SETFD, FD_CLOEXEC);
      ::fcntl(s_pipeio[Read], F_SETFL, O_NONBLOCK);
      return EventBackend::add(s_pipeio[Read], EPOLLIN, readstep); // watch for when execution stepper pipe has been triggered
    }
    return stepper_function != nullptr;
  }

  void step(void) noexcept
  {
    static const uint8_t dummydata = 0; // dummy content
    flaw(posix::write(s_pipeio[Write], &dummydata, 1) != 1, terminal::critical, /*std::exit(errno)*/,, // triggers execution stepper FD
         "Unable to trigger Object signal queue processor: %s", std::strerror(errno))
  }
}

#elif defined(__APPLE__)      /* Darwin 7+     */ || \
      defined(__FreeBSD__)    /* FreeBSD 4.1+  */ || \
      defined(__DragonFly__)  /* DragonFly BSD */ || \
      defined(__OpenBSD__)    /* OpenBSD 2.9+  */ || \
      defined(__NetBSD__)     /* NetBSD 2+     */

// BSD
#include <sys/time.h>
#include <sys/event.h>

// POSIX
#include <sys/socket.h>
#include <unistd.h>

// POSIX++
#include <cstdlib>
#include <cstdio>
#include <climits>
#include <cstring>

// STL
#include <vector>

// PDTK
#include <cxxutils/vterm.h>
#include <cxxutils/error_helpers.h>

lockable<std::unordered_multimap<posix::fd_t, EventBackend::callback_info_t>> EventBackend::queue;
std::list<std::pair<posix::fd_t, native_flags_t>> EventBackend::results;

struct EventBackend::platform_dependant // poll notification (epoll)
{
  posix::fd_t kq;
  std::vector<struct kevent> kinput;    // events we want to monitor
  std::vector<struct kevent> koutput;   // events that were triggered

  platform_dependant(void)
  {
    kq = posix::ignore_interruption(::kqueue);
    flaw(kq == posix::error_response, terminal::critical, std::exit(errno),,
         "Unable to create a new kqueue: %s", std::strerror(errno))
    kinput .reserve(1024);
    koutput.reserve(1024);
  }

  ~platform_dependant(void)
  {
    posix::close(kq);
  }
} EventBackend::s_platform;

static constexpr native_flags_t composite_flag(short filters, ushort flags) noexcept
  { return native_flags_t(uint16_t(flags) << 16) | native_flags_t(flags); }

static constexpr short extract_filter(native_flags_t flags) noexcept
  { return flags & 0xFFFF; }

static constexpr ushort extract_flags(native_flags_t flags) noexcept
  { return flags >> 16; }

bool EventBackend::add(posix::fd_t fd, native_flags_t flags, callback_t function) noexcept
{
  struct kevent ev;
  EV_SET(&ev, fd, extract_filter(flags), EV_ADD, extract_flags(flags), 0, nullptr);
  s_platform.kinput.push_back(ev);
  s_platform.koutput.resize(s_platform.kinput.size());
  queue.emplace(fd, (callback_info_t){flags, function});
  return true;
}

bool EventBackend::remove(posix::fd_t fd, native_flags_t flags) noexcept
{
  struct kevent ev;
  EV_SET(&ev, fd, extract_filter(flags), EV_ADD, extract_flags(flags), 0, nullptr);
  auto iter = s_platform.kinput.find(ev);
  if(iter == s_platform.kinput.end())
    return false;
  s_platform.kinput.erase(iter);
  return true;
}

bool EventBackend::poll(int timeout) noexcept
{
  uint32_t data;
  timespec tout;
  tout.tv_sec = timeout / 1000;
  tout.tv_nsec = (timeout % 1000) * 1000;

  int count = 0;
  count = kevent(s_platform.kq,
             s_platform.kinput.data(), s_platform.kinput.size(),
             s_platform.koutput.data(), s_platform.koutput.size(),
             &tout);

  if(count <= 0)
    return false;

  struct kevent* end = s_platform.koutput.data() + count;

  for(struct kevent* pos = s_platform.koutput.data(); pos != end; ++pos) // iterate through results
    results.emplace_back(std::make_pair(posix::fd_t(pos->ident), composite_flag(pos->filter, pos->flags)));
  return true;
}

#elif defined(__sun) && defined(__SVR4) // Solaris / OpenSolaris / OpenIndiana / illumos

#pragma message Not implemented, yet!
#pragma message See: http://docs.oracle.com/cd/E19253-01/816-5168/port-get-3c/index.html
#error The backend code in PDTK for Solaris / OpenSolaris / OpenIndiana / illumos is non-functional!  Please submit a patch!

// Solaris
#include <port.h>

// POSIX
#include <sys/socket.h>
#include <unistd.h>

// POSIX++
#include <cstdlib>
#include <cstdio>
#include <climits>
#include <cstring>

// STL
#include <vector>

// PDTK
#include <cxxutils/vterm.h>
#include <cxxutils/error_helpers.h>

struct platform_dependant
{
  posix::fd_t port;
  std::vector<port_event_t> pinput;   // events we want to monitor
  std::vector<port_event_t> poutput;  // events that were triggered

  platform_dependant(void)
  {
    port = ::port_create();
    flaw(port == posix::error_response, terminal::critical, std::exit(errno),,
         "Unable to create a new kqueue: %s", std::strerror(errno))
   pinput .reserve(1024);
   poutput.reserve(1024);
  }

  ~platform_dependant(void)
  {
    posix::close(port);
  }
} EventBackend::s_platform;

bool EventBackend::add(posix::fd_t fd, native_flags_t flags, callback_t function) noexcept
{
  port_event_t pev;

  s_platform.pinput.push_back(pev);
  queue.emplace(fd, (callback_info_t){flags, function});
  return true;
}

bool EventBackend::remove(posix::fd_t fd, native_flags_t flags) noexcept
{
  return false;
}

bool EventBackend::poll(int timeout) noexcept
{
  native_flags_t flags;
  uint_t count = 0;
  timespec tout;
  tout.tv_sec = timeout / 1000;
  tout.tv_nsec = (timeout % 1000) * 1000;

  if(::port_getn(s_platform.port,
                 &s_platform.pinput.data(), s_platform.pinput.size(),
                 s_platform.poutput, MAX_EVENTS, &count
                 &tout) == posix::error_response)
    return false;

  port_event_t* end = s_platform.poutput + count;

  for(port_event_t* pos = s_platform.poutput; pos != end; ++pos) // iterate through results
  {
    //flags = from_kevent(*pos);

    results.emplace(posix::fd_t(pos->ident), flags);
  }
  return true;
}

#elif defined(__minix) // MINIX
#error No event backend code exists in PDTK for MINIX!  Please submit a patch!

#elif defined(__QNX__) // QNX
// QNX docs: http://www.qnx.com/developers/docs/7.0.0/index.html#com.qnx.doc.neutrino.devctl/topic/about.html
#error No event backend code exists in PDTK for QNX!  Please submit a patch!

#elif defined(__hpux) // HP-UX
// see http://nixdoc.net/man-pages/HP-UX/man7/poll.7.html
// and https://www.freebsd.org/cgi/man.cgi?query=poll&sektion=7&apropos=0&manpath=HP-UX+11.22
// uses /dev/poll
#error No event backend code exists in PDTK for HP-UX!  Please submit a patch!

#include <sys/devpoll.h>

#elif defined(_AIX) // IBM AIX
// see https://www.ibm.com/support/knowledgecenter/ssw_aix_61/com.ibm.aix.basetrf1/pollset.htm
// uses pollset_* functions

#include <sys/poll.h>
#include <sys/pollset.h>

  pollset_t n;
  n = pollset_create(-1);
  pollset_destroy(n);

#error No event backend code exists in PDTK for IBM AIX!  Please submit a patch!

#elif defined(BSD)
#error Unrecognized BSD derivative!

#elif defined(__unix__)
#error Unrecognized UNIX variant!

#else
#error This platform is not supported.
#endif
<commit_msg>STL fix for kqueue backend<commit_after>#include "eventbackend.h"

#define MAX_EVENTS 1024

#if defined(__linux__)

// epoll needs kernel 2.5.44

// Linux
#include <sys/epoll.h>

// POSIX
#include <fcntl.h>

// POSIX++
#include <cstring> // for strerror()
#include <cstdlib> // for exit

// PDTK
#include <cxxutils/vterm.h>

struct EventBackend::platform_dependant // poll notification (epoll)
{
  posix::fd_t fd;
  struct epoll_event output[MAX_EVENTS];

  platform_dependant(void) noexcept
    : fd(posix::invalid_descriptor)
  {
    fd = ::epoll_create(MAX_EVENTS);
    flaw(fd == posix::invalid_descriptor, terminal::critical, std::exit(errno),,
         "Unable to create an instance of epoll! %s", std::strerror(errno))
  }

  ~platform_dependant(void) noexcept
  {
    posix::close(fd);
    fd = posix::invalid_descriptor;
  }

  bool add(posix::fd_t wd, native_flags_t flags) noexcept
  {
    struct epoll_event native_event;
    native_event.data.fd = wd;
    native_event.events = flags; // be sure to convert to native events
    return ::epoll_ctl(fd, EPOLL_CTL_ADD, wd, &native_event) == posix::success_response;
  }

  bool modify(posix::fd_t wd, native_flags_t flags) noexcept
  {
    struct epoll_event native_event;
    native_event.data.fd = wd;
    native_event.events = flags; // be sure to convert to native events

    return ::epoll_ctl(fd, EPOLL_CTL_MOD, wd, &native_event) == posix::success_response; // try to modify FD first
  }

  bool remove(posix::fd_t wd) noexcept
  {
    struct epoll_event event;
    return ::epoll_ctl(fd, EPOLL_CTL_DEL, wd, &event) == posix::success_response; // try to delete entry
  }
} EventBackend::s_platform;

lockable<std::unordered_multimap<posix::fd_t, EventBackend::callback_info_t>> EventBackend::queue;
std::list<std::pair<posix::fd_t, native_flags_t>> EventBackend::results;

bool EventBackend::add(posix::fd_t fd, native_flags_t flags, callback_t function) noexcept
{
  if(s_platform.add(fd, flags))
    queue.emplace(fd, (callback_info_t){flags, function});
  else if(errno == EEXIST && s_platform.modify(fd, flags)) // if entry exist and modification worked
  {
    bool found = false;
    auto entries = queue.equal_range(fd); // find modified entry!
    for(auto& pos = entries.first; pos != entries.second; ++pos)
    {
      found |= &(pos->second.function) == &function; // save if function was ever found
      if(&(pos->second.function) == &function) // if the FD is in the queue and it's callback function matches
        pos->second.flags |= flags; // simply modify the flags to include the current
    }

    if(!found) // function wasn't found
      queue.emplace(fd, (callback_info_t){flags, function}); // make a new entry for this FD
  }
  else // unable to add an entry or modify the entry for this FD!
    return false; // fail

  return true;
}

bool EventBackend::remove(posix::fd_t fd, native_flags_t flags) noexcept
{
  auto entries = queue.equal_range(fd); // find modified entry!
  for(auto& pos = entries.first; pos != entries.second; ++pos)
  {
    if(pos->second.flags == flags)
      queue.erase(pos);
    else if(pos->second.flags & flags)
    {
      pos->second.flags ^= pos->second.flags & flags;
      if(pos->second.flags) // if some flags are still set
        s_platform.modify(fd, pos->second.flags);
      else // if no flags are set then remove it completely
        s_platform.remove(fd);
    }
  }
  return true;
}

bool EventBackend::poll(int timeout) noexcept
{
  int count = ::epoll_wait(s_platform.fd, s_platform.output, MAX_EVENTS, timeout); // wait for new results
  results.clear(); // clear old results

  if(count == posix::error_response) // if error/timeout occurred
    return false; //fail

  const epoll_event* end = s_platform.output + count;
  for(epoll_event* pos = s_platform.output; pos != end; ++pos) // iterate through results
    results.emplace_back(std::make_pair(posix::fd_t(pos->data.fd), native_flags_t(pos->events))); // save result (in native format)
  return true;
}

namespace EventBackend
{
  static posix::fd_t s_pipeio[2] = { posix::invalid_descriptor }; //  execution stepper pipe
  static vfunc stepper_function = nullptr;
  enum {
    Read = 0,
    Write = 1,
  };

  void readstep(posix::fd_t fd, native_flags_t) noexcept
  {
    uint64_t discard;
    while(posix::read(fd, &discard, sizeof(discard)) != posix::error_response);
    stepper_function();
  }

  bool setstepper(vfunc function) noexcept
  {
    stepper_function = function;
    if(s_pipeio[Read] == posix::invalid_descriptor) // if execution stepper pipe  hasn't been initialized yet
    {
      flaw(::pipe(s_pipeio) == posix::error_response, terminal::critical, std::exit(errno), false,
           "Unable to create pipe for execution stepper: %s", std::strerror(errno))
      ::fcntl(s_pipeio[Read], F_SETFD, FD_CLOEXEC);
      ::fcntl(s_pipeio[Read], F_SETFL, O_NONBLOCK);
      return EventBackend::add(s_pipeio[Read], EPOLLIN, readstep); // watch for when execution stepper pipe has been triggered
    }
    return stepper_function != nullptr;
  }

  void step(void) noexcept
  {
    static const uint8_t dummydata = 0; // dummy content
    flaw(posix::write(s_pipeio[Write], &dummydata, 1) != 1, terminal::critical, /*std::exit(errno)*/,, // triggers execution stepper FD
         "Unable to trigger Object signal queue processor: %s", std::strerror(errno))
  }
}

#elif defined(__APPLE__)      /* Darwin 7+     */ || \
      defined(__FreeBSD__)    /* FreeBSD 4.1+  */ || \
      defined(__DragonFly__)  /* DragonFly BSD */ || \
      defined(__OpenBSD__)    /* OpenBSD 2.9+  */ || \
      defined(__NetBSD__)     /* NetBSD 2+     */

// BSD
#include <sys/time.h>
#include <sys/event.h>

// POSIX
#include <sys/socket.h>
#include <unistd.h>

// POSIX++
#include <cstdlib>
#include <cstdio>
#include <climits>
#include <cstring>

// STL
#include <vector>
#include <algorithm>
#include <iterator>

// PDTK
#include <cxxutils/vterm.h>
#include <cxxutils/error_helpers.h>

lockable<std::unordered_multimap<posix::fd_t, EventBackend::callback_info_t>> EventBackend::queue;
std::list<std::pair<posix::fd_t, native_flags_t>> EventBackend::results;

struct EventBackend::platform_dependant // poll notification (epoll)
{
  posix::fd_t kq;
  std::vector<struct kevent> kinput;    // events we want to monitor
  std::vector<struct kevent> koutput;   // events that were triggered

  platform_dependant(void)
  {
    kq = posix::ignore_interruption(::kqueue);
    flaw(kq == posix::error_response, terminal::critical, std::exit(errno),,
         "Unable to create a new kqueue: %s", std::strerror(errno))
    kinput .reserve(1024);
    koutput.reserve(1024);
  }

  ~platform_dependant(void)
  {
    posix::close(kq);
  }
} EventBackend::s_platform;

static constexpr native_flags_t composite_flag(short filters, ushort flags) noexcept
  { return native_flags_t(uint16_t(flags) << 16) | native_flags_t(flags); }

static constexpr short extract_filter(native_flags_t flags) noexcept
  { return flags & 0xFFFF; }

static constexpr ushort extract_flags(native_flags_t flags) noexcept
  { return flags >> 16; }

bool EventBackend::add(posix::fd_t fd, native_flags_t flags, callback_t function) noexcept
{
  struct kevent ev;
  EV_SET(&ev, fd, extract_filter(flags), EV_ADD, extract_flags(flags), 0, nullptr);
  s_platform.kinput.push_back(ev);
  s_platform.koutput.resize(s_platform.kinput.size());
  queue.emplace(fd, (callback_info_t){flags, function});
  return true;
}

bool EventBackend::remove(posix::fd_t fd, native_flags_t flags) noexcept
{
  struct kevent ev;
  EV_SET(&ev, fd, extract_filter(flags), EV_ADD, extract_flags(flags), 0, nullptr);

  auto iter = std::find(std::begin(s_platform.kinput), std::end(s_platform.kinput), ev);
  if(iter == std::end(s_platform.kinput))
    return false;
  s_platform.kinput.erase(iter);
  return true;
}

bool EventBackend::poll(int timeout) noexcept
{
  uint32_t data;
  timespec tout;
  tout.tv_sec = timeout / 1000;
  tout.tv_nsec = (timeout % 1000) * 1000;

  int count = 0;
  count = kevent(s_platform.kq,
             s_platform.kinput.data(), s_platform.kinput.size(),
             s_platform.koutput.data(), s_platform.koutput.size(),
             &tout);

  if(count <= 0)
    return false;

  struct kevent* end = s_platform.koutput.data() + count;

  for(struct kevent* pos = s_platform.koutput.data(); pos != end; ++pos) // iterate through results
    results.emplace_back(std::make_pair(posix::fd_t(pos->ident), composite_flag(pos->filter, pos->flags)));
  return true;
}

#elif defined(__sun) && defined(__SVR4) // Solaris / OpenSolaris / OpenIndiana / illumos

#pragma message Not implemented, yet!
#pragma message See: http://docs.oracle.com/cd/E19253-01/816-5168/port-get-3c/index.html
#error The backend code in PDTK for Solaris / OpenSolaris / OpenIndiana / illumos is non-functional!  Please submit a patch!

// Solaris
#include <port.h>

// POSIX
#include <sys/socket.h>
#include <unistd.h>

// POSIX++
#include <cstdlib>
#include <cstdio>
#include <climits>
#include <cstring>

// STL
#include <vector>

// PDTK
#include <cxxutils/vterm.h>
#include <cxxutils/error_helpers.h>

struct platform_dependant
{
  posix::fd_t port;
  std::vector<port_event_t> pinput;   // events we want to monitor
  std::vector<port_event_t> poutput;  // events that were triggered

  platform_dependant(void)
  {
    port = ::port_create();
    flaw(port == posix::error_response, terminal::critical, std::exit(errno),,
         "Unable to create a new kqueue: %s", std::strerror(errno))
   pinput .reserve(1024);
   poutput.reserve(1024);
  }

  ~platform_dependant(void)
  {
    posix::close(port);
  }
} EventBackend::s_platform;

bool EventBackend::add(posix::fd_t fd, native_flags_t flags, callback_t function) noexcept
{
  port_event_t pev;

  s_platform.pinput.push_back(pev);
  queue.emplace(fd, (callback_info_t){flags, function});
  return true;
}

bool EventBackend::remove(posix::fd_t fd, native_flags_t flags) noexcept
{
  return false;
}

bool EventBackend::poll(int timeout) noexcept
{
  native_flags_t flags;
  uint_t count = 0;
  timespec tout;
  tout.tv_sec = timeout / 1000;
  tout.tv_nsec = (timeout % 1000) * 1000;

  if(::port_getn(s_platform.port,
                 &s_platform.pinput.data(), s_platform.pinput.size(),
                 s_platform.poutput, MAX_EVENTS, &count
                 &tout) == posix::error_response)
    return false;

  port_event_t* end = s_platform.poutput + count;

  for(port_event_t* pos = s_platform.poutput; pos != end; ++pos) // iterate through results
  {
    //flags = from_kevent(*pos);

    results.emplace(posix::fd_t(pos->ident), flags);
  }
  return true;
}

#elif defined(__minix) // MINIX
#error No event backend code exists in PDTK for MINIX!  Please submit a patch!

#elif defined(__QNX__) // QNX
// QNX docs: http://www.qnx.com/developers/docs/7.0.0/index.html#com.qnx.doc.neutrino.devctl/topic/about.html
#error No event backend code exists in PDTK for QNX!  Please submit a patch!

#elif defined(__hpux) // HP-UX
// see http://nixdoc.net/man-pages/HP-UX/man7/poll.7.html
// and https://www.freebsd.org/cgi/man.cgi?query=poll&sektion=7&apropos=0&manpath=HP-UX+11.22
// uses /dev/poll
#error No event backend code exists in PDTK for HP-UX!  Please submit a patch!

#include <sys/devpoll.h>

#elif defined(_AIX) // IBM AIX
// see https://www.ibm.com/support/knowledgecenter/ssw_aix_61/com.ibm.aix.basetrf1/pollset.htm
// uses pollset_* functions

#include <sys/poll.h>
#include <sys/pollset.h>

  pollset_t n;
  n = pollset_create(-1);
  pollset_destroy(n);

#error No event backend code exists in PDTK for IBM AIX!  Please submit a patch!

#elif defined(BSD)
#error Unrecognized BSD derivative!

#elif defined(__unix__)
#error Unrecognized UNIX variant!

#else
#error This platform is not supported.
#endif
<|endoftext|>