void LUA_DISABLEPUBLISHER_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_DISABLEPUBLISHER,inArgs_DISABLEPUBLISHER[0],LUA_DISABLEPUBLISHER_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
result=ROS_server::removePublisher(topicName.c_str(),false);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (result==-1)
simSetLastError(LUA_DISABLEPUBLISHER_COMMAND, "Topic could not be unpublished."); // output an error
}
else
simSetLastError(LUA_DISABLEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
}
D.pushOutData(CLuaFunctionDataItem(result));
D.writeDataToLua(p);
}
void LUA_GETGRIPPERPROXSENSOR_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
float distance=100.0f;
if (D.readDataFromLua(p,inArgs_GETGRIPPERPROXSENSOR,inArgs_GETGRIPPERPROXSENSOR[0],LUA_GETGRIPPERPROXSENSOR_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
int sensorIndex=inData->at(1).intData[0];
if (k3Index!=-1)
{
if ( (sensorIndex>=0)&&(sensorIndex<2) )
{
float ptAndDist[4];
if (((simGetExplicitHandling(allK3s[k3Index].gripperDistanceSensorHandles[sensorIndex])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].gripperDistanceSensorHandles[sensorIndex],ptAndDist,NULL,NULL)>0))
distance=ptAndDist[3];
success=true;
}
else
simSetLastError(LUA_GETGRIPPERPROXSENSOR_COMMAND,"Invalid index.");
}
else
simSetLastError(LUA_GETGRIPPERPROXSENSOR_COMMAND,"Invalid Khepera3 handle.");
}
if (success)
D.pushOutData(CLuaFunctionDataItem(distance));
D.writeDataToLua(p);
}
void LUA_STOP_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_STOP,inArgs_STOP[0],LUA_STOP_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int port=inData->at(0).intData[0];
CSimxSocket* s=allConnections.getConnectionFromPort(port);
if (s!=NULL)
{
if (!s->getContinuousService())
{
allConnections.removeSocketConnection(s);
result=1;
}
else
simSetLastError(LUA_STOP_COMMAND,"Can't stop a continuous remote API server service."); // output an error
}
else
simSetLastError(LUA_STOP_COMMAND,"Invalid port number."); // output an error
}
D.pushOutData(CLuaFunctionDataItem(result));
D.writeDataToLua(p);
}
void LUA_STOP_COMMAND_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int result=-1; // error
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) 1 int as argument
CSimxSocket* s=allConnections.getConnectionFromPort(p->inputInt[0]);
if ( (s!=NULL)&&s->getActiveOnlyDuringSimulation() )
{
allConnections.removeSocketConnection(s);
result=1;
}
else
simSetLastError(LUA_STOP_COMMAND,"Invalid port number."); // output an error
}
else
simSetLastError(LUA_STOP_COMMAND,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_STOP_COMMAND,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
}
void LUA_WAKEPUBLISHER_CALLBACK(SScriptCallBack* p)
{
CScriptFunctionData D;
int result=-1;
if (D.readDataFromStack(p->stackID,inArgs_WAKEPUBLISHER,inArgs_WAKEPUBLISHER[0],LUA_WAKEPUBLISHER_COMMAND))
{
std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int publishCnt=inData->at(1).int32Data[0];
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
result=ROS_server::wakePublisher(topicName.c_str(),publishCnt);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (result==-2)
simSetLastError(LUA_WAKEPUBLISHER_COMMAND, "Topic could not be found."); // output an error
}
else
simSetLastError(LUA_WAKEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
}
D.pushOutData(CScriptFunctionDataItem(result));
D.writeDataToStack(p->stackID);
}
void LUA_GETENCODER_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
int encoder=0;
if (D.readDataFromLua(p,inArgs_GETENCODER,inArgs_GETENCODER[0],LUA_GETENCODER_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
int encoderIndex=inData->at(1).intData[0];
if (k3Index!=-1)
{
if ( (encoderIndex>=0)&&(encoderIndex<2) )
{
encoder=int(float(2764)*allK3s[k3Index].cumulativeMotorAngles[encoderIndex]/(2.0f*3.1415f)); // 2764 are the impulses per turn
success=true;
}
else
simSetLastError(LUA_GETENCODER_COMMAND,"Invalid index.");
}
else
simSetLastError(LUA_GETENCODER_COMMAND,"Invalid Khepera3 handle.");
}
if (success)
D.pushOutData(CLuaFunctionDataItem(encoder));
D.writeDataToLua(p);
}
void LUA_START_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_START,inArgs_START[0]-3,LUA_START_COMMAND)) // -3 because the last 3 args are optional
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int port=inData->at(0).intData[0];
int maxPacketSize=1300;
if (port<0)
maxPacketSize=3200000; // when using shared memory
bool debug=false;
bool triggerPreEnabled=false; // 3/3/2014
if (inData->size()>1)
maxPacketSize=inData->at(1).intData[0];
if (inData->size()>2)
debug=inData->at(2).boolData[0];
if (inData->size()>3)
triggerPreEnabled=inData->at(3).boolData[0];
if (port<0)
{ // when using shared memory
if (maxPacketSize<1000)
maxPacketSize=1000;
if (maxPacketSize>32000000)
maxPacketSize=32000000;
}
else
{ // when using sockets
if (maxPacketSize<200)
maxPacketSize=200;
if (maxPacketSize>30000)
maxPacketSize=30000;
}
CSimxSocket* s=allConnections.getConnectionFromPort(port);
if (s==NULL)
{
int prop,obj;
if (-1!=simGetScriptProperty(p->scriptID,&prop,&obj))
{
int scriptType=((prop|sim_scripttype_threaded)-sim_scripttype_threaded);
bool destroyAtSimulationEnd=( (scriptType==sim_scripttype_mainscript)||(scriptType==sim_scripttype_childscript)||(scriptType==sim_scripttype_jointctrlcallback)||(scriptType==sim_scripttype_contactcallback)||(scriptType==sim_scripttype_generalcallback) );
CSimxSocket* oneSocketConnection=new CSimxSocket(port,false,destroyAtSimulationEnd,debug,maxPacketSize,triggerPreEnabled); // 3/3/2014
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
result=1;
}
else
simSetLastError(LUA_START_COMMAND,"Unknown error."); // output an error
}
else
simSetLastError(LUA_START_COMMAND,"Invalid port number."); // output an error
}
D.pushOutData(CLuaFunctionDataItem(result));
D.writeDataToLua(p);
}
void LUA_SETGRIPPERGAP_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
if (D.readDataFromLua(p,inArgs_SETGRIPPERGAP,inArgs_SETGRIPPERGAP[0],LUA_SETGRIPPERGAP_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
int gap=inData->at(1).intData[0];
if (k3Index!=-1)
{
if (gap>170)
gap=170;
if (gap<0)
gap=0;
allK3s[k3Index].targetGripperGap=0.055f*float(gap)/170.0f;
success=true;
}
else
simSetLastError(LUA_SETGRIPPERGAP_COMMAND,"Invalid Khepera3 handle.");
}
D.pushOutData(CLuaFunctionDataItem(success));
D.writeDataToLua(p);
}
void LUA_SETVELOCITY_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
if (D.readDataFromLua(p,inArgs_SETVELOCITY,inArgs_SETVELOCITY[0],LUA_SETVELOCITY_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
float leftVel=inData->at(1).floatData[0];
float rightVel=inData->at(2).floatData[0];
if (k3Index!=-1)
{
if (leftVel>allK3s[k3Index].maxVelocity)
leftVel=allK3s[k3Index].maxVelocity;
if (leftVel<-allK3s[k3Index].maxVelocity)
leftVel=-allK3s[k3Index].maxVelocity;
if (rightVel>allK3s[k3Index].maxVelocity)
rightVel=allK3s[k3Index].maxVelocity;
if (rightVel<-allK3s[k3Index].maxVelocity)
rightVel=-allK3s[k3Index].maxVelocity;
allK3s[k3Index].targetVelocities[0]=leftVel;
allK3s[k3Index].targetVelocities[1]=rightVel;
success=true;
}
else
simSetLastError(LUA_SETVELOCITY_COMMAND,"Invalid Khepera3 handle.");
}
D.pushOutData(CLuaFunctionDataItem(success));
D.writeDataToLua(p);
}
void LUA_RESET_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_RESET,inArgs_RESET[0],LUA_RESET_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int port=inData->at(0).intData[0];
CSimxSocket* s=allConnections.getConnectionFromPort(port);
if (s!=NULL)
{
bool simulOnly=s->getActiveOnlyDuringSimulation();
bool continuous=s->getContinuousService();
bool debug=s->getDebug();
int maxPacketS=s->getMaxPacketSize();
bool triggerPreEnabled=s->getWaitForTriggerAuthorized();
// Kill the thread/connection:
allConnections.removeSocketConnection(s);
// Now create a similar thread/connection:
CSimxSocket* oneSocketConnection=new CSimxSocket(port,continuous,simulOnly,debug,maxPacketS,triggerPreEnabled);
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
result=1;
}
else
simSetLastError(LUA_RESET_COMMAND,"Invalid port number."); // output an error
}
D.pushOutData(CLuaFunctionDataItem(result));
D.writeDataToLua(p);
}
void LUA_SETARMPOSITION_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
if (D.readDataFromLua(p,inArgs_SETARMPOSITION,inArgs_SETARMPOSITION[0],LUA_SETARMPOSITION_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
int position=inData->at(1).intData[0];
if (k3Index!=-1)
{
if (position>900)
position=900;
if (position<300)
position=300;
allK3s[k3Index].targetArmPosition=(1.0f-float(position-300)/600.0f)*195.0f*3.1415f/180.0f;
success=true;
}
else
simSetLastError(LUA_SETARMPOSITION_COMMAND,"Invalid Khepera3 handle.");
}
D.pushOutData(CLuaFunctionDataItem(success));
D.writeDataToLua(p);
}
void LUA_END_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
simLockInterface(1);
int result=-1; // error
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) 1 int as argument
if ( (p->inputInt[0]>=0)&&(p->inputInt[0]<4)&&(startCountPerDevice[p->inputInt[0]]>0) )
{
startCountOverall--;
startCountPerDevice[p->inputInt[0]]--;
if (startCountPerDevice[p->inputInt[0]]==0)
{ // No one is using this device anymore... we remove it
EnterCriticalSection(&m_cs);
_allDevices[p->inputInt[0]]->Disconnect();
delete _allDevices[p->inputInt[0]];
_allDevices[p->inputInt[0]]=NULL;
LeaveCriticalSection(&m_cs);
}
if (startCountOverall==0)
killThread();
result=1;
}
else
simSetLastError(LUA_END,"Invalid device index."); // output an error
}
else
simSetLastError(LUA_END,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_END,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
simLockInterface(0);
}
void LUA_RESET_COMMAND_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int result=-1; // error
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) 1 int as argument
CSimxSocket* s=allConnections.getConnectionFromPort(p->inputInt[0]);
if (s!=NULL)
{
bool simulOnly=s->getActiveOnlyDuringSimulation();
bool debug=s->getDebug();
int maxPacketS=s->getMaxPacketSize();
bool triggerPreEnabled=s->getWaitForTriggerAuthorized();
// Kill the thread/connection:
allConnections.removeSocketConnection(s);
// Now create a similar thread/connection:
CSimxSocket* oneSocketConnection=new CSimxSocket(p->inputInt[0],simulOnly,debug,maxPacketS,triggerPreEnabled);
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
result=1;
}
else
simSetLastError(LUA_RESET_COMMAND,"Invalid port number."); // output an error
}
else
simSetLastError(LUA_RESET_COMMAND,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_RESET_COMMAND,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
}
void LUA_END_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int result=-1; // error
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) 1 int as argument
if ( (p->inputInt[0]<4)&&(startCountPerDevice[p->inputInt[0]]>0) )
{
startCountOverall--;
startCountPerDevice[p->inputInt[0]]--;
if (startCountPerDevice[p->inputInt[0]]==0)
{
EnterCriticalSection(&m_cs);
deinitCapture(p->inputInt[0]);
delete[] captureInfo[p->inputInt[0]].mTargetBuf;
openCaptureDevices[p->inputInt[0]]=false;
LeaveCriticalSection(&m_cs);
}
if (startCountOverall==0)
killThread();
result=1;
}
else
simSetLastError(LUA_END,"Invalid device index."); // output an error
}
else
simSetLastError(LUA_END,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_END,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
}
void LUA_ENABLESUBSCRIBER_CALLBACK(SScriptCallBack* p)
{
CScriptFunctionData D;
int result=-1;
if (D.readDataFromStack(p->stackID,inArgs_ENABLESUBSCRIBER,inArgs_ENABLESUBSCRIBER[0]-2,LUA_ENABLESUBSCRIBER_COMMAND)) // -2 because the last two args are optional
{
std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int queueSize=inData->at(1).int32Data[0];
int streamCmd=inData->at(2).int32Data[0];
int auxInt1=inData->at(3).int32Data[0];
int auxInt2=inData->at(4).int32Data[0];
std::string auxString(inData->at(5).stringData[0]);
int callbackTag_before=-1; // no callback (default)
int callbackTag_after=-1; // no callback (default)
if (inData->size()>6)
callbackTag_before=inData->at(6).int32Data[0];
if (inData->size()>7)
callbackTag_after=inData->at(7).int32Data[0];
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
result=ROS_server::addSubscriber(topicName.c_str(),queueSize,streamCmd,auxInt1,auxInt2,auxString.c_str(),callbackTag_before,callbackTag_after);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (result==-1)
simSetLastError(LUA_ENABLESUBSCRIBER_COMMAND, "Topic could not be subscribed."); // output an error
}
else
simSetLastError(LUA_ENABLESUBSCRIBER_COMMAND, "Invalid topic name."); // output an error
}
D.pushOutData(CScriptFunctionDataItem(result));
D.writeDataToStack(p->stackID);
}
void LUA_INFO_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
char infoString[200];
infoString[0]=0;
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) one int as argument
if ( (countCaptureDevices()>p->inputInt[0])&&(p->inputInt[0]>=0)&&(p->inputInt[0]<4) )
{
getCaptureDeviceName(p->inputInt[0],infoString,200);
}
else
simSetLastError(LUA_INFO,"Wrong index."); // output an error
}
else
simSetLastError(LUA_INFO,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_INFO,"Not enough arguments."); // output an error
// Now we prepare the return value:
int l=int(strlen(infoString));
if (l!=0)
{
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_string; // The return value is a string
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputChar=(simChar*)simCreateBuffer(l+1); // 1 string return value
for (int i=0;i<l;i++)
p->outputChar[i]=infoString[i];
p->outputChar[l]=0;
}
else
p->outputArgCount=0; // no return values (nil)
}
void LUA_ENABLEPUBLISHER_CALLBACK(SScriptCallBack* p)
{
CScriptFunctionData D;
std::string effectiveTopicName;
if (D.readDataFromStack(p->stackID,inArgs_ENABLEPUBLISHER,inArgs_ENABLEPUBLISHER[0]-1,LUA_ENABLEPUBLISHER_COMMAND)) // -1 because the last argument is optional
{
std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int queueSize=inData->at(1).int32Data[0];
int streamCmd=inData->at(2).int32Data[0];
int auxInt1=inData->at(3).int32Data[0];
int auxInt2=inData->at(4).int32Data[0];
std::string auxString(inData->at(5).stringData[0]);
int publishCnt=0; // 0 is the default value for this optional argument
if (inData->size()>6)
publishCnt=inData->at(6).int32Data[0];
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
effectiveTopicName=ROS_server::addPublisher(topicName.c_str(),queueSize,streamCmd,auxInt1,auxInt2,auxString.c_str(),publishCnt);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (effectiveTopicName.length()==0)
simSetLastError(LUA_ENABLEPUBLISHER_COMMAND, "Topic could not be published."); // output an error
}
else
simSetLastError(LUA_ENABLEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
}
if (effectiveTopicName.size()!=0)
D.pushOutData(CScriptFunctionDataItem(effectiveTopicName));
D.writeDataToStack(p->stackID);
}
void LUA_SET_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
simLockInterface(1);
int result=-1; // error
if (p->inputArgCount>2)
{ // Ok, we have at least 3 input argument
if ( (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[1*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[2*2+0]==sim_lua_arg_bool) )
{ // Ok, we have (at least) 2 ints and a bool as argument
if ( (p->inputInt[0]>=0)&&(p->inputInt[0]<4)&&(startCountPerDevice[p->inputInt[0]]>0) )
{ // Ok, we have an "open" device here
_allDeviceData[p->inputInt[0]].leds=BYTE(p->inputInt[1]);
_allDeviceData[p->inputInt[0]].rumble=(p->inputBool[1]!=0);
result=1;
}
else
simSetLastError(LUA_SET,"Invalid device index."); // output an error
}
else
simSetLastError(LUA_SET,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_SET,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
simLockInterface(0);
}
void LUA_GETLINESENSOR_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
float intensity=0.0f; // no detection
if (D.readDataFromLua(p,inArgs_GETLINESENSOR,inArgs_GETLINESENSOR[0],LUA_GETLINESENSOR_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
int sensorIndex=inData->at(1).intData[0];
if (k3Index!=-1)
{
if ( (sensorIndex>=0)&&(sensorIndex<2) )
{
float* auxValues=NULL;
int* auxValuesCount=NULL;
if (simReadVisionSensor(allK3s[k3Index].colorSensorHandles[sensorIndex],&auxValues,&auxValuesCount)>=0)
{
if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
intensity=auxValues[10];
simReleaseBuffer((char*)auxValues);
simReleaseBuffer((char*)auxValuesCount);
}
success=true;
}
else
simSetLastError(LUA_GETLINESENSOR_COMMAND,"Invalid index.");
}
else
simSetLastError(LUA_GETLINESENSOR_COMMAND,"Invalid Khepera3 handle.");
}
if (success)
D.pushOutData(CLuaFunctionDataItem(intensity));
D.writeDataToLua(p);
}
void v_repImportUrdfCallback(SLuaCallBack* p)
{
if (p->inputArgCount == 10) {
simChar* name = v_repImportUrdf(p->inputChar,p->inputBool[0],p->inputBool[1],p->inputBool[2],p->inputBool[3],
p->inputBool[4],p->inputBool[5],p->inputBool[6],p->inputBool[7],p->inputBool[8]);
p->outputArgCount = 1;
p->outputArgTypeAndSize = (simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt));
p->outputArgTypeAndSize[0] = sim_lua_arg_string;
p->outputArgTypeAndSize[1] = 1;
p->outputChar = (simChar*)simCreateBuffer(strlen(name)+1);
memcpy((void*)p->outputChar, (void*)name, strlen(name)+1);
} else {
simSetLastError("simExtImportUrdfCallback","Wrong number of arguments.");
p->outputArgCount = 0;
}
}
void LUA_DISABLESUBSCRIBER_CALLBACK(SScriptCallBack* p)
{
CScriptFunctionData D;
bool result=false;
if (D.readDataFromStack(p->stackID,inArgs_DISABLESUBSCRIBER,inArgs_DISABLESUBSCRIBER[0],LUA_DISABLESUBSCRIBER_COMMAND))
{
std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
result=ROS_server::removeSubscriber(inData->at(0).int32Data[0]);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (!result)
simSetLastError(LUA_DISABLESUBSCRIBER_COMMAND, "Topic could not be unsubscribed."); // output an error
}
D.pushOutData(CScriptFunctionDataItem(result));
D.writeDataToStack(p->stackID);
}
void LUA_GETSENSORDATA_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command ("simExtSkeleton_getSensorData")
p->outputArgCount=0;
CLuaFunctionData D;
// If successful the command will return an interger (result), a float table of size 3 (data), and a float (distance). If the command is not successful, it will not return anything
bool commandWasSuccessful=false;
int returnResult;
std::vector<float> returnData;
float returnDistance;
if (D.readDataFromLua(p,inArgs_GETSENSORDATA,inArgs_GETSENSORDATA[0],LUA_GETSENSORDATA_COMMAND))
{ // above function reads in the expected arguments. If the arguments are wrong, it returns false and outputs a message to the simulation status bar
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int sensorIndex=inData->at(0).intData[0]; // the first argument
std::vector<float>& floatParameters=inData->at(1).floatData; // the second argument
std::vector<int>& intParameters=inData->at(2).intData; // the third argument
// Now you can do something with above's arguments. For example:
if ((sensorIndex>=0)&&(sensorIndex<10))
{
commandWasSuccessful=true;
returnResult=1;
returnData.push_back(1.0f);
returnData.push_back(2.0f);
returnData.push_back(3.0f);
returnDistance=59.0f;
}
else
simSetLastError(LUA_GETSENSORDATA_COMMAND,"Invalid sensor index."); // output an error message to the simulator's status bar
}
if (commandWasSuccessful)
{ // prepare the return values:
D.pushOutData(CLuaFunctionDataItem(returnResult));
D.pushOutData(CLuaFunctionDataItem(returnData));
D.pushOutData(CLuaFunctionDataItem(returnDistance));
}
D.writeDataToLua(p);
}
void LUA_SETENCODERS_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
if (D.readDataFromLua(p,inArgs_SETENCODERS,inArgs_SETENCODERS[0],LUA_SETENCODERS_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int k3Index=getK3IndexFromHandle(inData->at(0).intData[0]);
int leftEncoder=inData->at(1).intData[0];
int rightEncoder=inData->at(2).intData[0];
if (k3Index!=-1)
{
allK3s[k3Index].cumulativeMotorAngles[0]=float(leftEncoder)*(2.0f*3.1415f)/float(2764); // 2764 are the impulses per turn
allK3s[k3Index].cumulativeMotorAngles[1]=float(rightEncoder)*(2.0f*3.1415f)/float(2764);
success=true;
}
else
simSetLastError(LUA_SETENCODERS_COMMAND,"Invalid Khepera3 handle.");
}
D.pushOutData(CLuaFunctionDataItem(success));
D.writeDataToLua(p);
}
void LUA_DESTROY_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool success=false;
if (D.readDataFromLua(p,inArgs_DESTROY,inArgs_DESTROY[0],LUA_DESTROY_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int handle=inData->at(0).intData[0];
int k3Index=getK3IndexFromHandle(handle);
if (k3Index>=0)
{
if (allK3s[k3Index].waitUntilZero!=NULL)
allK3s[k3Index].waitUntilZero[0]=0; // free the blocked thread
allK3s.erase(allK3s.begin()+k3Index);
success=true;
}
else
simSetLastError(LUA_DESTROY_COMMAND,"Invalid Khepera3 handle.");
}
D.pushOutData(CLuaFunctionDataItem(success));
D.writeDataToLua(p);
}
void LUA_START_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int result=-1; // error
int returnResolution[2]={0,0};
if (p->inputArgCount>2)
{ // Ok, we have at least 3 input argument
if ( (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[1*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[2*2+0]==sim_lua_arg_int) )
{ // Ok, we have (at least) 3 ints as argument
if ( (countCaptureDevices()>p->inputInt[0])&&(p->inputInt[0]>=0)&&(p->inputInt[0]<4) )
{
if (!openCaptureDevices[p->inputInt[0]])
{ // We can set the new resolution
bool goOn=true;
if (startCountOverall==0)
{ // Launch the thread!
_camThreadLaunched=false;
CreateThread(NULL,0,_camThread,NULL,THREAD_PRIORITY_NORMAL,NULL);
while (!_camThreadLaunched)
Sleep(2);
if (deviceCount<1)
{
simSetLastError(LUA_START,"ESCAPI initialization failure or no devices found."); // output an error
killThread();
goOn=false;
}
}
if (goOn)
{
captureInfo[p->inputInt[0]].mWidth=p->inputInt[1];
captureInfo[p->inputInt[0]].mHeight=p->inputInt[2];
captureInfo[p->inputInt[0]].mTargetBuf=new int[p->inputInt[1]*p->inputInt[2]];
if (initCapture(p->inputInt[0],&captureInfo[p->inputInt[0]])!=0)
{
doCapture(p->inputInt[0]);
openCaptureDevices[p->inputInt[0]]=true;
returnResolution[0]=p->inputInt[1];
returnResolution[1]=p->inputInt[2];
result=1; // success!
startCountOverall++;
startCountPerDevice[p->inputInt[0]]++;
}
else
{
delete[] captureInfo[p->inputInt[0]].mTargetBuf;
simSetLastError(LUA_START,"Device may already be in use."); // output an error
}
}
}
else
{ // We have to retrieve the current resolution
returnResolution[0]=captureInfo[p->inputInt[0]].mWidth;
returnResolution[1]=captureInfo[p->inputInt[0]].mHeight;
result=0;
startCountOverall++;
startCountPerDevice[p->inputInt[0]]++;
}
}
else
simSetLastError(LUA_START,"Invalid device index."); // output an error
}
else
simSetLastError(LUA_START,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_START,"Not enough arguments."); // output an error
// Now we prepare the return value:
if (result>-1)
{
p->outputArgCount=3; // 3 return values
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*1+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*1+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*2+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*2+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(3*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
p->outputInt[1]=returnResolution[0]; // This is the int value we want to return
p->outputInt[2]=returnResolution[1]; // This is the int value we want to return
}
else
{
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
}
}
void LUA_GRAB_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int retVal=0; // Means error
if (p->inputArgCount>1)
{ // Ok, we have at least 2 input argument
if ( (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)&&(p->inputArgTypeAndSize[1*2+0]==sim_lua_arg_int) )
{ // Ok, we have (at least) 2 ints as argument
if ( (countCaptureDevices()>p->inputInt[0])&&(p->inputInt[0]>=0)&&(p->inputInt[0]<4) )
{
if (startCountPerDevice[p->inputInt[0]]>0)
{
if (openCaptureDevices[p->inputInt[0]])
{
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
int t=simGetObjectType(p->inputInt[1]);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (t==sim_object_visionsensor_type)
{
int r[2]={0,0};
simGetVisionSensorResolution(p->inputInt[1],r);
if ( (r[0]==captureInfo[p->inputInt[0]].mWidth)&&(r[1]==captureInfo[p->inputInt[0]].mHeight) )
{
float* buff=new float[r[0]*r[1]*3];
for (int i=0;i<r[1];i++)
{
int y0=r[0]*i;
int y1=r[0]*(r[1]-i-1);
for (int j=0;j<r[0];j++)
{ // Info is provided as BGR!! (and not RGB)
buff[3*(y0+j)+0]=float(((BYTE*)captureInfo[p->inputInt[0]].mTargetBuf)[4*(y1+j)+2])/255.0f;
buff[3*(y0+j)+1]=float(((BYTE*)captureInfo[p->inputInt[0]].mTargetBuf)[4*(y1+j)+1])/255.0f;
buff[3*(y0+j)+2]=float(((BYTE*)captureInfo[p->inputInt[0]].mTargetBuf)[4*(y1+j)+0])/255.0f;
}
}
simSetVisionSensorImage(p->inputInt[1],buff);
delete[] buff;
retVal=1;
}
else
simSetLastError(LUA_GRAB,"Resolutions do not match."); // output an error
}
else
simSetLastError(LUA_GRAB,"Invalid vision sensor handle."); // output an error
}
else
simSetLastError(LUA_GRAB,"Resolution was not set."); // output an error
}
else
simSetLastError(LUA_GRAB,"simExtCamStart was not called."); // output an error
}
else
simSetLastError(LUA_GRAB,"Wrong index."); // output an error
}
else
simSetLastError(LUA_GRAB,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_GRAB,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(retVal)); // 1 int return value
p->outputInt[0]=retVal; // This is the int value we want to return
}
bool CScriptFunctionData::_readData(int stack,const int* expectedArguments,int requiredArgumentCount,const char* functionName,const char* argumentText1,const char* argumentText2,std::vector<CScriptFunctionDataItem>& inOutData)
{ // use this when reading data from a script from inside of a custom script function call
inOutData.clear();
int argCnt=simGetStackSize(stack);
if (argCnt<requiredArgumentCount)
{
std::ostringstream str;
str << "Not enough " << argumentText1;
simSetLastError(functionName,str.str().c_str());
return(false);
}
for (int i=0;i<argCnt;i++)
{
if (i>=expectedArguments[0])
break;
bool done=false;
simMoveStackItemToTop(stack,0);
if (simIsStackValueNull(stack)==1)
{
// is nil explicitely allowed?
if (expectedArguments[1+i*2+0]&SIM_SCRIPT_ARG_NULL_ALLOWED)
{ // yes. This is for an argument that can optionally also be nil.
CScriptFunctionDataItem dat;
inOutData.push_back(dat);
done=true;
}
else
{ // no
if (int(inOutData.size())<requiredArgumentCount)
{
std::ostringstream str;
str << argumentText2 << i+1 << " is not correct.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
break; // this argument is nil, so it is like inexistant. But we also won't explore any more arguments, we have enough.
}
}
if (!done)
{
int tableSize=simGetStackTableInfo(stack,0);
bool expectingATable=(((expectedArguments[1+i*2+0]|SIM_SCRIPT_ARG_NULL_ALLOWED)-SIM_SCRIPT_ARG_NULL_ALLOWED)&sim_script_arg_table)!=0;
if ( (tableSize>=0)!=expectingATable )
{
std::ostringstream str;
str << argumentText2 << i+1 << " is not correct.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
int expectingType=(((expectedArguments[1+i*2+0]|SIM_SCRIPT_ARG_NULL_ALLOWED)-SIM_SCRIPT_ARG_NULL_ALLOWED)|sim_script_arg_table)-sim_script_arg_table;
if (expectingATable)
{ // we have a table
int infoType=0;
if (expectingType==sim_script_arg_null)
infoType=1;
if (expectingType==sim_script_arg_bool)
infoType=3;
if (expectingType==sim_script_arg_float)
infoType=2;
if (expectingType==sim_script_arg_int32)
infoType=2;
if (expectingType==sim_script_arg_string)
infoType=4;
if (expectingType==sim_script_arg_charbuff)
infoType=4;
if (expectingType==sim_script_arg_double)
infoType=2;
if (simGetStackTableInfo(stack,infoType)<1)
{ // table content cannot be converted
std::ostringstream str;
str << argumentText2 << i+1 << " is not correct.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
if ( (tableSize<expectedArguments[1+i*2+1])&&(expectedArguments[1+i*2+1]!=0) )
{
std::ostringstream str;
str << argumentText2 << i+1 << " is not correct (wrong table size).";
simSetLastError(functionName,str.str().c_str());
return(false);
}
else
{
int t=expectingType;
int itemCnt=tableSize;
if (t==sim_script_arg_null)
{
CScriptFunctionDataItem* a=new CScriptFunctionDataItem();
a->setNilTable(itemCnt);
CScriptFunctionDataItem dat;
dat.setNilTable(itemCnt);
inOutData.push_back(dat);
}
if (t==sim_script_arg_bool)
{
std::vector<bool> vect;
simUnfoldStackTable(stack); // this removes the table and exposes the inside
for (int j=0;j<itemCnt;j++)
{
simBool val;
simGetStackBoolValue(stack,&val);
vect.insert(vect.begin(),val!=0);
simPopStackItem(stack,2);
}
simPushTableOntoStack(stack); // empty table, will be removed at the end of the loop
CScriptFunctionDataItem dat(vect);
inOutData.push_back(dat);
}
if (t==sim_script_arg_int32)
{
std::vector<int> vect;
if (itemCnt>0)
{
vect.resize(itemCnt);
simGetStackInt32Table(stack,&vect[0],itemCnt);
}
CScriptFunctionDataItem dat(vect);
inOutData.push_back(dat);
}
if (t==sim_script_arg_float)
{
std::vector<float> vect;
if (itemCnt>0)
{
vect.resize(itemCnt);
simGetStackFloatTable(stack,&vect[0],itemCnt);
}
CScriptFunctionDataItem dat(vect);
inOutData.push_back(dat);
}
if (t==sim_script_arg_double)
{
std::vector<double> vect;
if (itemCnt>0)
{
vect.resize(itemCnt);
simGetStackDoubleTable(stack,&vect[0],itemCnt);
}
CScriptFunctionDataItem dat(vect);
inOutData.push_back(dat);
}
if (t==sim_script_arg_string)
{
std::vector<std::string> vect;
simUnfoldStackTable(stack); // this removes the table and exposes the inside
for (int j=0;j<itemCnt;j++)
{
int l;
char* str=simGetStackStringValue(stack,&l);
std::string str2(str); // treat it as a char string, not buffer
simReleaseBuffer(str);
vect.insert(vect.begin(),str2);
simPopStackItem(stack,2);
}
simPushTableOntoStack(stack); // empty table, will be removed at the end of the loop
CScriptFunctionDataItem dat(vect);
inOutData.push_back(dat);
}
if (t==sim_script_arg_charbuff)
{
std::ostringstream str;
str << argumentText2 << i+1 << " cannot be a table.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
}
}
else
{ // we do not have a table
int t=expectingType;
bool failedMsgAndLeave=false;
if (t==sim_script_arg_null)
{
if (simIsStackValueNull(stack)>0)
{
CScriptFunctionDataItem dat;
inOutData.push_back(dat);
}
else
failedMsgAndLeave=true;
}
if (t==sim_script_arg_bool)
{
simBool val=0;
if (simGetStackBoolValue(stack,&val)==1)
{
CScriptFunctionDataItem dat(val!=0);
inOutData.push_back(dat);
}
else
failedMsgAndLeave=true;
}
if (t==sim_script_arg_int32)
{
int val=0;
if (simGetStackInt32Value(stack,&val)==1)
{
CScriptFunctionDataItem dat(val);
inOutData.push_back(dat);
}
else
failedMsgAndLeave=true;
}
if (t==sim_script_arg_float)
{
float val=0.0;
if (simGetStackFloatValue(stack,&val)==1)
{
CScriptFunctionDataItem dat(val);
inOutData.push_back(dat);
}
else
failedMsgAndLeave=true;
}
if (t==sim_script_arg_double)
{
double val=0.0;
if (simGetStackDoubleValue(stack,&val)==1)
{
CScriptFunctionDataItem dat(val);
inOutData.push_back(dat);
}
else
failedMsgAndLeave=true;
}
if (t==sim_script_arg_string)
{
int l;
char* str=simGetStackStringValue(stack,&l);
if (str!=NULL)
{
std::string str2(str);
simReleaseBuffer(str);
CScriptFunctionDataItem dat(str2); // treat it as a char string, not buffer
inOutData.push_back(dat);
}
else
failedMsgAndLeave=true;
}
if (t==sim_script_arg_charbuff)
{
int l;
char* str=simGetStackStringValue(stack,&l);
if (str!=NULL)
{
if ( (l<expectedArguments[1+i*2+1])&&(expectedArguments[1+i*2+1]!=0) )
{
simReleaseBuffer(str);
std::ostringstream str;
str << argumentText2 << i+1 << " is not correct (wrong buffer size).";
simSetLastError(functionName,str.str().c_str());
return(false);
}
else
{
CScriptFunctionDataItem dat(str,l);
inOutData.push_back(dat);
simReleaseBuffer(str);
}
}
else
failedMsgAndLeave=true;
}
if (failedMsgAndLeave)
{
std::ostringstream str;
str << argumentText2 << i+1 << " is not correct.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
}
}
simPopStackItem(stack,1);
}
return(true);
}
void LUA_STATUS_COMMAND_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int result=-1; // error or no connection at given index
int info[5];
int clientVersion=-1;
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) 1 int as argument
CSimxSocket* s=allConnections.getConnectionFromPort(p->inputInt[0]);
// CSimxSocket* s=allConnections.getConnectionAtIndex(p->inputInt[0]);
if (s!=NULL)
{
result=s->getStatus();
s->getInfo(info);
clientVersion=s->getClientVersion();
}
}
else
simSetLastError(LUA_STATUS_COMMAND,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_STATUS_COMMAND,"Not enough arguments."); // output an error
// Now we prepare the return value:
if (result>-1)
{
p->outputArgCount=4; // 4 return values
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*1+0]=sim_lua_arg_int|sim_lua_arg_table; // The return value is an int table
p->outputArgTypeAndSize[2*1+1]=5; // Table size
p->outputArgTypeAndSize[2*2+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*2+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*3+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*3+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(8*sizeof(result)); // total of 8 ints
p->outputInt[0]=result; // This is the int value we want to return (arg 1)
// Now the table values:
for (int i=0;i<5;i++)
p->outputInt[1+i]=info[i];
p->outputInt[6]=SIMX_VERSION; // This is the server version info (arg 3)
p->outputInt[7]=clientVersion; // This is the client version info (arg 4)
}
else
{
p->outputArgCount=4; // 4 return values
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*1+0]=sim_lua_arg_nil; // The return value is nil
p->outputArgTypeAndSize[2*1+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*2+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*2+1]=1; // Not used (table size if the return value was a table)
p->outputArgTypeAndSize[2*3+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*3+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(3*sizeof(result)); // 3 int return value
p->outputInt[0]=result; // This is the int value we want to return
p->outputInt[1]=SIMX_VERSION; // This is the second int value we want to return
p->outputInt[2]=clientVersion; // This is the third int value we want to return
}
}
bool CLuaFunctionData::readDataFromLua(const SLuaCallBack* p,const int* expectedArguments,int requiredArgumentCount,const char* functionName)
{ // use this when reading data from Lua from inside of a custom Lua function call
_inData.clear();
int argCnt=p->inputArgCount;
if (argCnt<requiredArgumentCount)
{
simSetLastError(functionName,"Not enough arguments.");
return(false);
}
int boolArgInd=0;
int intArgInd=0;
int floatArgInd=0;
int doubleArgInd=0;
int charArgInd=0;
int charBuffArgInd=0;
for (int i=0;i<argCnt;i++)
{
if (i>=expectedArguments[0])
break;
bool done=false;
if (p->inputArgTypeAndSize[i*2+0]==sim_lua_arg_nil)
{
// is nil explicitely allowed?
if (expectedArguments[1+i*2+0]&SIM_LUA_ARG_NIL_ALLOWED)
{ // yes. This is for an argument that can optionally also be nil.
CLuaFunctionDataItem dat;
_inData.push_back(dat);
done=true;
}
else
{ // no
if (int(_inData.size())<requiredArgumentCount)
{
std::ostringstream str;
str << "Argument " << i+1 << " is not correct.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
break; // this argument is nil, so it is like inexistant. But we also won't explore any more arguments, we have enough.
}
}
if (!done)
{
if (p->inputArgTypeAndSize[i*2+0]!=((expectedArguments[1+i*2+0]|SIM_LUA_ARG_NIL_ALLOWED)-SIM_LUA_ARG_NIL_ALLOWED))
{
std::ostringstream str;
str << "Argument " << i+1 << " is not correct.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
if (p->inputArgTypeAndSize[i*2+0]&sim_lua_arg_table)
{ // we have a table
if ( (p->inputArgTypeAndSize[i*2+1]<expectedArguments[1+i*2+1])&&(expectedArguments[1+i*2+1]!=0) )
{
std::ostringstream str;
str << "Argument " << i+1 << " is not correct (wrong table size).";
simSetLastError(functionName,str.str().c_str());
return(false);
}
else
{
int t=p->inputArgTypeAndSize[i*2+0]-sim_lua_arg_table;
int itemCnt=p->inputArgTypeAndSize[i*2+1];
if (t==sim_lua_arg_nil)
{
CLuaFunctionDataItem* a=new CLuaFunctionDataItem();
a->setNilTable(itemCnt);
CLuaFunctionDataItem dat;
dat.setNilTable(itemCnt);
_inData.push_back(dat);
}
if (t==sim_lua_arg_bool)
{
std::vector<bool> vect;
for (int j=0;j<itemCnt;j++)
vect.push_back(p->inputBool[boolArgInd++]!=0);
CLuaFunctionDataItem dat(vect);
_inData.push_back(dat);
}
if (t==sim_lua_arg_int)
{
std::vector<int> vect;
for (int j=0;j<itemCnt;j++)
vect.push_back(p->inputInt[intArgInd++]);
CLuaFunctionDataItem dat(vect);
_inData.push_back(dat);
}
if (t==sim_lua_arg_float)
{
std::vector<float> vect;
for (int j=0;j<itemCnt;j++)
vect.push_back(p->inputFloat[floatArgInd++]);
CLuaFunctionDataItem dat(vect);
_inData.push_back(dat);
}
if (t==sim_lua_arg_double)
{
std::vector<double> vect;
for (int j=0;j<itemCnt;j++)
vect.push_back(p->inputDouble[doubleArgInd++]);
CLuaFunctionDataItem dat(vect);
_inData.push_back(dat);
}
if (t==sim_lua_arg_string)
{
std::vector<std::string> vect;
for (int j=0;j<itemCnt;j++)
{
std::string str(p->inputChar+charArgInd);
vect.push_back(str);
charArgInd+=int(strlen(p->inputChar+charArgInd))+1;
}
CLuaFunctionDataItem dat(vect);
_inData.push_back(dat);
}
if (t==sim_lua_arg_charbuff)
{
std::ostringstream str;
str << "Argument " << i+1 << " cannot be a table.";
simSetLastError(functionName,str.str().c_str());
return(false);
}
}
}
else
{ // we do not have a table
int t=p->inputArgTypeAndSize[i*2+0];
if (t==sim_lua_arg_nil)
{
CLuaFunctionDataItem dat;
_inData.push_back(dat);
}
if (t==sim_lua_arg_bool)
{
CLuaFunctionDataItem dat(p->inputBool[boolArgInd++]!=0);
_inData.push_back(dat);
}
if (t==sim_lua_arg_int)
{
CLuaFunctionDataItem dat(p->inputInt[intArgInd++]);
_inData.push_back(dat);
}
if (t==sim_lua_arg_float)
{
CLuaFunctionDataItem dat(p->inputFloat[floatArgInd++]);
_inData.push_back(dat);
}
if (t==sim_lua_arg_double)
{
CLuaFunctionDataItem dat(p->inputDouble[doubleArgInd++]);
_inData.push_back(dat);
}
if (t==sim_lua_arg_string)
{
CLuaFunctionDataItem dat(std::string(p->inputChar+charArgInd));
charArgInd+=int(strlen(p->inputChar+charArgInd))+1;
_inData.push_back(dat);
}
if (t==sim_lua_arg_charbuff)
{
if ( (p->inputArgTypeAndSize[i*2+1]<expectedArguments[1+i*2+1])&&(expectedArguments[1+i*2+1]!=0) )
{
std::ostringstream str;
str << "Argument " << i+1 << " is not correct (wrong buffer size).";
simSetLastError(functionName,str.str().c_str());
return(false);
}
else
{
CLuaFunctionDataItem dat(p->inputCharBuff+charBuffArgInd,p->inputArgTypeAndSize[i*2+1]);
charBuffArgInd+=p->inputArgTypeAndSize[i*2+1];
_inData.push_back(dat);
}
}
}
}
}
return(true);
}
void LUA_START_COMMAND_CALLBACK(SLuaCallBack* p)
{ // the callback function of the new Lua command
int result=-1; // error
if (p->inputArgCount>0)
{ // Ok, we have at least 1 input argument
if (p->inputArgTypeAndSize[0*2+0]==sim_lua_arg_int)
{ // Ok, we have (at least) 1 int as argument
bool err=false;
int maxPacketSize=1300;
if (p->inputInt[0]<0)
maxPacketSize=3200000; // when using shared memory
bool debug=false;
bool triggerPreEnabled=false; // 3/3/2014
if (p->inputArgCount>1)
{
if (p->inputArgTypeAndSize[1*2+0]==sim_lua_arg_int)
{
maxPacketSize=p->inputInt[1];
if (p->inputInt[0]<0)
{ // when using shared memory
if (maxPacketSize<1000)
maxPacketSize=1000;
if (maxPacketSize>32000000)
maxPacketSize=32000000;
}
else
{ // when using sockets
if (maxPacketSize<200)
maxPacketSize=200;
if (maxPacketSize>30000)
maxPacketSize=30000;
}
}
else
err=true;
if (p->inputArgCount>2)
{
if (p->inputArgTypeAndSize[2*2+0]==sim_lua_arg_bool)
debug=(p->inputBool[0]!=0);
else
err=true;
// 3/3/2014
if (p->inputArgCount>3)
{
if (p->inputArgTypeAndSize[3*2+0]==sim_lua_arg_bool)
triggerPreEnabled=(p->inputBool[1]!=0);
else
err=true;
}
}
}
if (!err)
{
CSimxSocket* s=allConnections.getConnectionFromPort(p->inputInt[0]);
if (s==NULL)
{
CSimxSocket* oneSocketConnection=new CSimxSocket(p->inputInt[0],true,debug,maxPacketSize,triggerPreEnabled); // 3/3/2014
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
result=1;
}
else
simSetLastError(LUA_START_COMMAND,"Invalid port number."); // output an error
}
else
simSetLastError(LUA_START_COMMAND,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_START_COMMAND,"Wrong argument type/size."); // output an error
}
else
simSetLastError(LUA_START_COMMAND,"Not enough arguments."); // output an error
// Now we prepare the return value:
p->outputArgCount=1; // 1 return value
p->outputArgTypeAndSize=(simInt*)simCreateBuffer(p->outputArgCount*2*sizeof(simInt)); // x return values takes x*2 simInt for the type and size buffer
p->outputArgTypeAndSize[2*0+0]=sim_lua_arg_int; // The return value is an int
p->outputArgTypeAndSize[2*0+1]=1; // Not used (table size if the return value was a table)
p->outputInt=(simInt*)simCreateBuffer(1*sizeof(result)); // 1 int return value
p->outputInt[0]=result; // This is the int value we want to return
}
