void LUA_STATUS_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
std::vector<int> info(5,0);
int clientVersion=-1;
char connectedMachineIP[200]="";
if (D.readDataFromLua(p,inArgs_STATUS,inArgs_STATUS[0],LUA_STATUS_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
int port=inData->at(0).intData[0];
CSimxSocket* s=allConnections.getConnectionFromPort(port);
if (s!=NULL)
{
result=s->getStatus();
s->getInfo(&info[0]);
clientVersion=s->getClientVersion();
strcpy(connectedMachineIP,s->getConnectedMachineIP().c_str());
}
}
D.pushOutData(CLuaFunctionDataItem(result));
if (result>-1)
D.pushOutData(CLuaFunctionDataItem(info));
else
D.pushOutData(CLuaFunctionDataItem());
D.pushOutData(CLuaFunctionDataItem(SIMX_VERSION));
D.pushOutData(CLuaFunctionDataItem(clientVersion));
if (result>-1)
D.pushOutData(CLuaFunctionDataItem(std::string(connectedMachineIP)));
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_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_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_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_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_WAKEPUBLISHER_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_WAKEPUBLISHER,inArgs_WAKEPUBLISHER[0],LUA_WAKEPUBLISHER_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int publishCnt=inData->at(1).intData[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(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_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_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_CREATE_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int handle=-1;
if (D.readDataFromLua(p,inArgs_CREATE,inArgs_CREATE[0],LUA_CREATE_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
sK3 k3;
k3.k3BaseHandle=p->objectID;
handle=nextK3Handle++;
k3.handle=handle;
k3.waitUntilZero=NULL;
for (unsigned int i=0;i<2;i++)
k3.wheelMotorHandles[i]=inData->at(0).intData[i];
for (unsigned int i=0;i<2;i++)
k3.colorSensorHandles[i]=inData->at(1).intData[i];
for (unsigned int i=0;i<9;i++)
k3.irSensorHandles[i]=inData->at(2).intData[i];
for (unsigned int i=0;i<5;i++)
k3.usSensorHandles[i]=inData->at(3).intData[i];
for (unsigned int i=0;i<6;i++)
k3.armMotorHandles[i]=inData->at(4).intData[i];
for (unsigned int i=0;i<3;i++)
k3.fingerMotorHandles[i]=inData->at(5).intData[i];
for (unsigned int i=0;i<2;i++)
k3.gripperDistanceSensorHandles[i]=inData->at(6).intData[i];
for (unsigned int i=0;i<2;i++)
k3.gripperColorSensorHandles[i]=inData->at(7).intData[i];
k3.uiHandle=inData->at(8).intData[0];
k3.maxVelocity=6.283f;
k3.maxAcceleration=25.0f;
k3.maxArmAcceleration=0.5f;
k3.targetVelocities[0]=0.0f;
k3.targetVelocities[1]=0.0f;
k3.currentVelocities[0]=0.0f;
k3.currentVelocities[1]=0.0f;
k3.cumulativeMotorAngles[0]=0.0f;
k3.cumulativeMotorAngles[1]=0.0f;
k3.previousMotorAngles[0]=0.0f;
k3.previousMotorAngles[1]=0.0f;
k3.targetArmPosition=0.0f;
k3.currentArmPosition=0.0f;
k3.currentArmVelocity=0.0f;
k3.targetGripperGap=0.055f;
k3.currentGripperGap=0.055f;
allK3s.push_back(k3);
}
D.pushOutData(CLuaFunctionDataItem(handle));
D.writeDataToLua(p);
}
void LUA_ENABLESUBSCRIBER_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_ENABLESUBSCRIBER,inArgs_ENABLESUBSCRIBER[0]-2,LUA_ENABLESUBSCRIBER_COMMAND)) // -2 because the last two args are optional
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int queueSize=inData->at(1).intData[0];
int streamCmd=inData->at(2).intData[0];
int auxInt1=inData->at(3).intData[0];
int auxInt2=inData->at(4).intData[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).intData[0];
if (inData->size()>7)
callbackTag_after=inData->at(7).intData[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(CLuaFunctionDataItem(result));
D.writeDataToLua(p);
}
void LUA_DISABLESUBSCRIBER_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
bool result=false;
if (D.readDataFromLua(p,inArgs_DISABLESUBSCRIBER,inArgs_DISABLESUBSCRIBER[0],LUA_DISABLESUBSCRIBER_COMMAND))
{
std::vector<CLuaFunctionDataItem>* 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).intData[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(CLuaFunctionDataItem(result));
D.writeDataToLua(p);
}
void LUA_ENABLEPUBLISHER_CALLBACK(SLuaCallBack* p)
{
p->outputArgCount=0;
CLuaFunctionData D;
std::string effectiveTopicName;
if (D.readDataFromLua(p,inArgs_ENABLEPUBLISHER,inArgs_ENABLEPUBLISHER[0]-1,LUA_ENABLEPUBLISHER_COMMAND)) // -1 because the last argument is optional
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int queueSize=inData->at(1).intData[0];
int streamCmd=inData->at(2).intData[0];
int auxInt1=inData->at(3).intData[0];
int auxInt2=inData->at(4).intData[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).intData[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(CLuaFunctionDataItem(effectiveTopicName));
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_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 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_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p)
{ // the callback function of the new Lua command ("simExtSkeleton_getSensorData")
// return Lua Table or arrays containing position, torque, torque minus motor force, timestamp, FRI state
try {
if (!kukaPluginPG) {
BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n";
CLuaFunctionData data;
if (data.readDataFromLua(p,inArgs_KUKA_LBR_IIWA_START,inArgs_KUKA_LBR_IIWA_START[0],LUA_KUKA_LBR_IIWA_START_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=data.getInDataPtr();
std::vector<std::string> JointHandles;
for (size_t i=0;i<inData->at(0).stringData.size();i++)
{
JointHandles.push_back(std::string(inData->at(0).stringData[i].c_str()));
}
std::string RobotTipHandle (inData->at(1 ).stringData[0]);
std::string RobotTargetHandle (inData->at(2 ).stringData[0]);
std::string RobotTargetBaseHandle (inData->at(3 ).stringData[0]);
std::string RobotModel (inData->at(4 ).stringData[0]);
std::string LocalZMQAddress (inData->at(5 ).stringData[0]);
std::string RemoteZMQAddress (inData->at(6 ).stringData[0]);
std::string LocalHostKukaKoniUDPAddress (inData->at(7 ).stringData[0]);
std::string LocalHostKukaKoniUDPPort (inData->at(8 ).stringData[0]);
std::string RemoteHostKukaKoniUDPAddress (inData->at(9 ).stringData[0]);
std::string RemoteHostKukaKoniUDPPort (inData->at(10).stringData[0]);
std::string KukaCommandMode (inData->at(11).stringData[0]);
std::string KukaMonitorMode (inData->at(12).stringData[0]);
std::string IKGroupName (inData->at(13).stringData[0]);
kukaPluginPG=std::make_shared<grl::vrep::KukaVrepPlugin>(
std::make_tuple(
JointHandles ,
RobotTipHandle ,
RobotTargetHandle ,
RobotTargetBaseHandle ,
RobotModel ,
LocalZMQAddress ,
RemoteZMQAddress ,
LocalHostKukaKoniUDPAddress ,
LocalHostKukaKoniUDPPort ,
RemoteHostKukaKoniUDPAddress ,
RemoteHostKukaKoniUDPPort ,
KukaCommandMode ,
KukaMonitorMode ,
IKGroupName
)
);
kukaPluginPG->construct();
} else {
/// @todo report an error?
// use default params
kukaPluginPG=std::make_shared<grl::vrep::KukaVrepPlugin>();
kukaPluginPG->construct();
}
}
} catch (const boost::exception& e){
// log the error and print it to the screen, don't release the exception
std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
simAddStatusbarMessage( initerr.c_str());
BOOST_LOG_TRIVIAL(error) << initerr;
kukaPluginPG.reset();
} catch (const std::exception& e){
// log the error and print it to the screen, don't release the exception
std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e));
simAddStatusbarMessage( initerr.c_str());
BOOST_LOG_TRIVIAL(error) << initerr;
kukaPluginPG.reset();
} catch (...){
// log the error and print it to the screen, don't release the exception
std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast<std::string>(__LINE__) + "\n");
simAddStatusbarMessage( initerr.c_str());
BOOST_LOG_TRIVIAL(error) << initerr;
kukaPluginPG.reset();
}
}
void LUA_DECIMATE_CALLBACK(SLuaCallBack* p)
{ // keep for backward compatibility
p->outputArgCount=0;
CLuaFunctionData D;
int result=-1;
if (D.readDataFromLua(p,inArgs_DECIMATE,inArgs_DECIMATE[0],LUA_DECIMATE_COMMAND))
{
std::vector<CLuaFunctionDataItem>* inData=D.getInDataPtr();
float* outV;
int outVL;
int* outI;
int outIL;
float percent=float(inData->at(3).intData[0])/float(inData->at(1).intData.size()/3);
int res=simGetDecimatedMesh(&inData->at(0).floatData[0],inData->at(0).floatData.size(),&inData->at(1).intData[0],inData->at(1).intData.size(),&outV,&outVL,&outI,&outIL,percent,0,NULL);
if (res>0)
{
std::vector<float> v2(outV,outV+outVL);
std::vector<int> i2(outI,outI+outIL);
simReleaseBuffer((simChar*)outV);
simReleaseBuffer((simChar*)outI);
D.pushOutData(CLuaFunctionDataItem(v2));
D.pushOutData(CLuaFunctionDataItem(i2));
}
/*
Mesh mesh;
Decimater decimater(mesh);
HModQuadric hModQuadric;
decimater.add(hModQuadric);
decimater.module(hModQuadric).unset_max_err();
// HModRoundness hModRoundness;
// decimater.add(hModRoundness);
// decimater.module(hModRoundness).set_binary(false);
// HModHausdorff hModHausdorff;
// decimater.add(hModHausdorff);
// decimater.module(hModHausdorff).set_binary(false);
// HModAspectRatio hModAspectRatio;
// decimater.add(hModAspectRatio);
// decimater.module(hModAspectRatio).set_binary(false);
// HModNormalDeviation hModNormalDeviation;
// decimater.add(hModNormalDeviation);
// decimater.module(hModNormalDeviation).set_binary(false);
std::vector<Mesh::VertexHandle> vhandles;
for (int i=0;i<int(inData->at(0).floatData.size()/3);i++)
vhandles.push_back(mesh.add_vertex(Mesh::Point(inData->at(0).floatData[3*i+0],inData->at(0).floatData[3*i+1],inData->at(0).floatData[3*i+2])));
std::vector<Mesh::VertexHandle> face_vhandles;
for (int i=0;i<int(inData->at(1).intData.size()/3);i++)
{
face_vhandles.clear();
face_vhandles.push_back(vhandles[inData->at(1).intData[3*i+0]]);
face_vhandles.push_back(vhandles[inData->at(1).intData[3*i+1]]);
face_vhandles.push_back(vhandles[inData->at(1).intData[3*i+2]]);
mesh.add_face(face_vhandles);
}
decimater.initialize();
decimater.decimate_to_faces(inData->at(2).intData[0],inData->at(3).intData[0]);
mesh.garbage_collection();
std::vector<float> newVertices;
Mesh::VertexHandle vh;
OpenMesh::Vec3f v;
for (int i=0;i<int(mesh.n_vertices());i++)
{
vh = Mesh::VertexHandle(i);
v = mesh.point(vh);
newVertices.push_back(v[0]);
newVertices.push_back(v[1]);
newVertices.push_back(v[2]);
}
std::vector<int> newIndices;
Mesh::FaceHandle fh;
OpenMesh::ArrayItems::Face f;
for (int i=0;i<int(mesh.n_faces());i++)
{
fh = Mesh::FaceHandle(i);
mesh.cfv_iter(fh);
OpenMesh::PolyConnectivity::ConstFaceVertexIter cfv_it=mesh.cfv_iter(fh);
newIndices.push_back(cfv_it->idx());
++cfv_it;
newIndices.push_back(cfv_it->idx());
++cfv_it;
newIndices.push_back(cfv_it->idx());
}
D.pushOutData(CLuaFunctionDataItem(newVertices));
D.pushOutData(CLuaFunctionDataItem(newIndices));
*/
}
D.writeDataToLua(p);
}
