int urdfLink::scaleShapeIfRequired(int shapeHandle,float scalingFactors[3])
{ // in future there will be a non-iso scaling function for objects in V-REP, but until then...
if ( (scalingFactors[0]*scalingFactors[1]*scalingFactors[2]>0.99999f)&&(scalingFactors[0]>0.0f)&&(scalingFactors[1]>0.0f) )
return(shapeHandle); // no scaling required!
if (fabs(scalingFactors[0])<0.00001f)
scalingFactors[0]=0.00001f*scalingFactors[0]/fabs(scalingFactors[0]);
if (fabs(scalingFactors[1])<0.00001f)
scalingFactors[1]=0.00001f*scalingFactors[1]/fabs(scalingFactors[1]);
if (fabs(scalingFactors[2])<0.00001f)
scalingFactors[2]=0.00001f*scalingFactors[2]/fabs(scalingFactors[2]);
int newShapeHandle=shapeHandle;
float* vertices;
int verticesSize;
int* indices;
int indicesSize;
if (simGetShapeMesh(shapeHandle,&vertices,&verticesSize,&indices,&indicesSize,NULL)!=-1)
{
// Scale the vertices:
C7Vector tr;
simGetObjectPosition(shapeHandle,-1,tr.X.data);
C3Vector euler;
simGetObjectOrientation(shapeHandle,-1,euler.data);
tr.Q.setEulerAngles(euler);
for (int i=0;i<verticesSize/3;i++)
{
C3Vector v(vertices+3*i);
v*=tr;
v(0)*=scalingFactors[0];
v(1)*=scalingFactors[1];
v(2)*=scalingFactors[2];
vertices[3*i+0]=v(0);
vertices[3*i+1]=v(1);
vertices[3*i+2]=v(2);
}
// Flip the triangles (if needed)
if (scalingFactors[0]*scalingFactors[1]*scalingFactors[2]<0.0f)
{
for (int i=0;i<indicesSize/3;i++)
{
int tmp=indices[3*i+0];
indices[3*i+0]=indices[3*i+1];
indices[3*i+1]=tmp;
}
}
// Remove the old shape and create a new one with the scaled data:
simRemoveObject(shapeHandle);
newShapeHandle=simCreateMeshShape(2,20.0f*piValue/180.0f,vertices,verticesSize,indices,indicesSize,NULL);
simReleaseBuffer((char*)vertices);
simReleaseBuffer((char*)indices);
}
return(newShapeHandle);
}
//
//Generate image message
//
void generate_image_message(sensor_msgs::Image& image_msg)
{
int size[2], data_len;
float* image_buf = simGetVisionSensorImage(camera_handle);
simGetVisionSensorResolution(camera_handle, size);
image_msg.encoding = sensor_msgs::image_encodings::RGB8;
image_msg.height = size[1];
image_msg.width = size[0];
image_msg.step = image_msg.width * 3; //image stride in bytes
data_len = image_msg.step * image_msg.height;
image_msg.data.resize(data_len);
image_msg.is_bigendian = 0;
int msg_idx, buf_idx;
for(unsigned int i = 0; i < image_msg.height; i++)
{
for(unsigned int j = 0; j < image_msg.step; j++)
{
msg_idx = (image_msg.height - i - 1) * image_msg.step + j;
buf_idx = i * image_msg.step + j;
image_msg.data[msg_idx] = (unsigned char)(image_buf[buf_idx] * 255);
}
}
simReleaseBuffer((char*)image_buf);
}
RTC::ReturnCode_t GyroRTC::onExecute(RTC::UniqueId ec_id)
{
simInt bufSize;
simChar* pBuffer = simTubeRead(m_tubeHandle, &bufSize);
if (pBuffer == NULL) {
return RTC::RTC_OK;
}
float_byte buffer;
int data_size = bufSize / 4;
if (data_size != 3) {
std::cout << "[GyroRTC] Invalid data size (" << data_size << "!=3)" << std::endl;
return RTC::RTC_OK;
}
float time = simGetSimulationTime();
long sec = floor(time);
long nsec = (time - sec) * 1000*1000*1000;
m_gyro.tm.sec = sec;
m_gyro.tm.nsec = nsec;
float x[3];
for(int i = 0;i < 3;i++) {
for(int j = 0;j < 4;j++) {
buffer.byte_value[j] = pBuffer[i*4+j];
}
x[i] = buffer.float_value;
}
m_gyro.data.avx = x[0];
m_gyro.data.avy = x[1];
m_gyro.data.avz = x[2];
m_gyroOut.write();
simReleaseBuffer((simChar*)pBuffer);
return RTC::RTC_OK;
}
float CK3::getLineSensor(int index)
{
_initialize();
if ( (index<0)||(index>=2) )
return(-1.0f); // error
float* auxValues=NULL;
int* auxValuesCount=NULL;
float retVal=NO_DETECTION_VALUE_INTENSITY;
if (simReadVisionSensor(_k3ColorSensorHandles[index],&auxValues,&auxValuesCount)>=0)
{
if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
retVal=auxValues[10];
simReleaseBuffer((char*)auxValues);
simReleaseBuffer((char*)auxValuesCount);
}
return(retVal);
}
void CSubscriberData::appendStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
std::string theNewString;
int stringLength;
char* stringSignal=simGetStringSignal(auxStr.c_str(),&stringLength);
if (stringSignal!=NULL)
{
theNewString=std::string(stringSignal,stringLength);
simReleaseBuffer(stringSignal);
}
theNewString+=std::string(&sig->data[0],sig->data.length());
simSetStringSignal(auxStr.c_str(),theNewString.c_str(),theNewString.length());
}
void simulatorLoop()
{ // The main application loop (excluding the GUI part)
static bool wasRunning=false;
int auxValues[4];
int messageID=0;
int dataSize;
if (autoStart)
{
simStartSimulation();
autoStart=false;
}
while (messageID!=-1)
{
simChar* data=simGetSimulatorMessage(&messageID,auxValues,&dataSize);
if (messageID!=-1)
{
if (messageID==sim_message_simulation_start_resume_request)
simStartSimulation();
if (messageID==sim_message_simulation_pause_request)
simPauseSimulation();
if (messageID==sim_message_simulation_stop_request)
simStopSimulation();
if (data!=NULL)
simReleaseBuffer(data);
}
}
// Handle a running simulation:
if ( (simGetSimulationState()&sim_simulation_advancing)!=0 )
{
wasRunning=true;
if ( (simGetRealTimeSimulation()!=1)||(simIsRealTimeSimulationStepNeeded()==1) )
{
if ((simHandleMainScript()&sim_script_main_script_not_called)==0)
simAdvanceSimulationByOneStep();
if ((simStopDelay>0)&&(simGetSimulationTime()>=float(simStopDelay)/1000.0f))
{
simStopDelay=0;
simStopSimulation();
}
}
}
else
{
if (wasRunning&&autoQuit)
{
wasRunning=false;
simQuitSimulator(true); // will post the quit command
}
}
}
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 CBubbleRobDialog::refresh()
{ // This refreshed the dialog's items:
int lastSel=simGetObjectLastSelection();
m_bubbleRobList.ResetContent();
for (int i=0;i<CAccess::bubbleRobContainer->getCount();i++)
{
CBubbleRob* bubbleRob=CAccess::bubbleRobContainer->getFromIndex(i);
int sceneObjectHandle=bubbleRob->getAssociatedObject();
std::string txt="Object: ";
char* name=NULL;
if (sceneObjectHandle!=-1)
name=simGetObjectName(sceneObjectHandle);
if (name==NULL)
txt+="Unassociated";
else
{
txt+="Associated with ";
txt+=name;
simReleaseBuffer(name);
}
int index=m_bubbleRobList.AddString(txt.c_str());
m_bubbleRobList.SetItemData(index,(DWORD)bubbleRob->getID());
}
CBubbleRob* taggedObj=NULL;
if (lastSel!=-1)
{
taggedObj=CAccess::bubbleRobContainer->getFromAssociatedObject(lastSel);
if (taggedObj==NULL)
selectObjectInList(-1);
else
selectObjectInList(taggedObj->getID());
}
else
selectObjectInList(-1);
taggedObj=CAccess::bubbleRobContainer->getFromID(getSelectedObjectInList());
m_maxVelocity.EnableWindow(taggedObj!=NULL);
if (taggedObj!=NULL)
{
CString tmp;
tmp.Format("%0.2f",120.0f*taggedObj->getMaxVelocity()/3.1415f); // display it in RPM
m_maxVelocity.SetWindowText(tmp);
}
else
m_maxVelocity.SetWindowText("");
}
void CSimxContainer::executeCommands(CSimxContainer* outputContainer,CSimxSocket* sock)
{
if (outputContainer->getCommandCount()==0)
{ // Make sure the output buffer was already sent and is empty
outputContainer->setMessageID(_messageID);
outputContainer->setDataTimeStamp(_dataTimeStamp);
int simState=simGetSimulationState();
BYTE serverState=0;
outputContainer->setDataServerTimeStamp(int(simGetSystemTime()*1000.1f));
if (simState!=sim_simulation_stopped)
{
serverState|=1; // simulation is not stopped
if (simState==sim_simulation_paused)
serverState|=2; // simulation is paused
}
if (simGetRealTimeSimulation()>0)
serverState|=4;
int editModeType;
simGetIntegerParameter(sim_intparam_edit_mode_type,&editModeType);
serverState|=(editModeType<<3);
int sceneUniqueID;
simGetIntegerParameter(sim_intparam_scene_unique_id,&sceneUniqueID);
outputContainer->setSceneID(WORD(sceneUniqueID));
outputContainer->setServerState(serverState);
// Prepare for correct error reporting:
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_report);
char* err=simGetLastError(); // just clear the last error
if (err!=NULL)
simReleaseBuffer(err);
// Execute pending commands:
for (unsigned int i=0;i<_allCommands.size();i++)
{
CSimxCmd* outputCmd=_allCommands[i]->execute(sock,_otherSideIsBigEndian);
if (outputCmd!=NULL) // gradual or split commands are not added here!
outputContainer->addCommand(outputCmd,false);
}
// Restore previous error report mode:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved);
_removeNonContinuousCommands(); // simx_opmode_oneshot_split commands are also kept!
}
}
void CMtbRobotDialog::updateObjectsInList()
{
ui->qqRobotList->clear();
for (int i=0;i<CAccess::mtbRobotContainer->getCount();i++)
{
CMtbRobot* mtbRobot=CAccess::mtbRobotContainer->getFromIndex(i);
int sceneObjectHandle=mtbRobot->getAssociatedObject();
std::string txt;
char* name=NULL;
if (sceneObjectHandle!=-1)
name=simGetObjectName(sceneObjectHandle);
if (name==NULL)
txt="Error";
else
{
txt=name;
simReleaseBuffer(name);
}
QListWidgetItem* itm=new QListWidgetItem(txt.c_str());
itm->setData(Qt::UserRole,QVariant(mtbRobot->getID()));
itm->setFlags(Qt::ItemIsSelectable|Qt::ItemIsEnabled);
ui->qqRobotList->addItem(itm);
}
}
robot::robot(std::string filename,bool hideCollisionLinks,bool hideJoints,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg,bool centerAboveGround,bool makeModel,bool noSelfCollision,bool positionCtrl): filenameAndPath(filename)
{
printToConsole("URDF import operation started.");
openFile();
readJoints();
readLinks();
readSensors();
createJoints(hideJoints,positionCtrl);
createLinks(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg);
createSensors();
std::vector<int> parentlessObjects;
std::vector<int> allShapes;
std::vector<int> allObjects;
std::vector<int> allSensors;
for (int i=0;i<int(vLinks.size());i++)
{
if (simGetObjectParent(vLinks[i]->nLinkVisual)==-1)
parentlessObjects.push_back(vLinks[i]->nLinkVisual);
allObjects.push_back(vLinks[i]->nLinkVisual);
allShapes.push_back(vLinks[i]->nLinkVisual);
if (vLinks[i]->nLinkCollision!=-1)
{
if (simGetObjectParent(vLinks[i]->nLinkCollision)==-1)
parentlessObjects.push_back(vLinks[i]->nLinkCollision);
allObjects.push_back(vLinks[i]->nLinkCollision);
allShapes.push_back(vLinks[i]->nLinkCollision);
}
}
for (int i=0;i<int(vJoints.size());i++)
{
if (vJoints[i]->nJoint!=-1)
{
if (simGetObjectParent(vJoints[i]->nJoint)==-1)
parentlessObjects.push_back(vJoints[i]->nJoint);
allObjects.push_back(vJoints[i]->nJoint);
}
}
for (int i=0;i<int(vSensors.size());i++)
{
if (vSensors[i]->nSensor!=-1)
{
if (simGetObjectParent(vSensors[i]->nSensor)==-1)
parentlessObjects.push_back(vSensors[i]->nSensor);
allObjects.push_back(vSensors[i]->nSensor);
allSensors.push_back(vSensors[i]->nSensor);
}
if (vSensors[i]->nSensorAux!=-1)
{
allObjects.push_back(vSensors[i]->nSensorAux);
allSensors.push_back(vSensors[i]->nSensorAux);
}
}
// If we want to alternate respondable mask:
if (!noSelfCollision)
{
for (int i=0;i<int(parentlessObjects.size());i++)
setLocalRespondableMaskCummulative_alternate(parentlessObjects[i],true);
}
// Now center the model:
if (centerAboveGround)
{
bool firstValSet=false;
C3Vector minV,maxV;
for (int shNb=0;shNb<int(allShapes.size());shNb++)
{
float* vertices;
int verticesSize;
int* indices;
int indicesSize;
if (simGetShapeMesh(allShapes[shNb],&vertices,&verticesSize,&indices,&indicesSize,NULL)!=-1)
{
C7Vector tr;
simGetObjectPosition(allShapes[shNb],-1,tr.X.data);
C3Vector euler;
simGetObjectOrientation(allShapes[shNb],-1,euler.data);
tr.Q.setEulerAngles(euler);
for (int i=0;i<verticesSize/3;i++)
{
C3Vector v(vertices+3*i);
v*=tr;
if (!firstValSet)
{
minV=v;
maxV=v;
firstValSet=true;
}
else
{
minV.keepMin(v);
maxV.keepMax(v);
}
}
simReleaseBuffer((char*)vertices);
simReleaseBuffer((char*)indices);
}
}
C3Vector shiftAmount((minV+maxV)*-0.5f);
shiftAmount(2)+=(maxV(2)-minV(2))*0.5f;
for (int i=0;i<int(parentlessObjects.size());i++)
{
C3Vector p;
simGetObjectPosition(parentlessObjects[i],-1,p.data);
p+=shiftAmount;
simSetObjectPosition(parentlessObjects[i],-1,p.data);
}
}
// Now create a model bounding box if that makes sense:
if ((makeModel)&&(parentlessObjects.size()==1))
{
int p=simGetModelProperty(parentlessObjects[0]);
p|=sim_modelproperty_not_model;
simSetModelProperty(parentlessObjects[0],p-sim_modelproperty_not_model);
for (int i=0;i<int(allObjects.size());i++)
{
if (allObjects[i]!=parentlessObjects[0])
{
int p=simGetObjectProperty(allObjects[i]);
simSetObjectProperty(allObjects[i],p|sim_objectproperty_selectmodelbaseinstead);
}
}
for (int i=0;i<int(allSensors.size());i++)
{
if (allSensors[i]!=parentlessObjects[0])
{
int p=simGetObjectProperty(allSensors[i]);
simSetObjectProperty(allSensors[i],p|sim_objectproperty_dontshowasinsidemodel); // sensors are usually large and it is ok if they do not appear as inside of the model bounding box!
}
}
}
// Now select all new objects:
simRemoveObjectFromSelection(sim_handle_all,-1);
for (int i=0;i<int(allObjects.size());i++)
simAddObjectToSelection(sim_handle_single,allObjects[i]);
printToConsole("URDF import operation finished.\n\n");
}
// This is the plugin start routine:
VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt)
{ // This is called just once, at the start of V-REP
// Dynamically load and bind V-REP functions:
char curDirAndFile[1024];
#ifdef _WIN32
#ifdef QT_COMPIL
_getcwd(curDirAndFile, sizeof(curDirAndFile));
#else
GetModuleFileName(NULL,curDirAndFile,1023);
PathRemoveFileSpec(curDirAndFile);
#endif
#elif defined (__linux) || defined (__APPLE__)
getcwd(curDirAndFile, sizeof(curDirAndFile));
#endif
std::string currentDirAndPath(curDirAndFile);
std::string temp(currentDirAndPath);
#ifdef _WIN32
temp+="\\v_rep.dll";
#elif defined (__linux)
temp+="/libv_rep.so";
#elif defined (__APPLE__)
temp+="/libv_rep.dylib";
#endif /* __linux || __APPLE__ */
vrepLib=loadVrepLibrary(temp.c_str());
if (vrepLib==NULL)
{
std::cout << "Error, could not find or correctly load the V-REP library. Cannot start 'RemoteApi' plugin.\n";
return(0); // Means error, V-REP will unload this plugin
}
if (getVrepProcAddresses(vrepLib)==0)
{
std::cout << "Error, could not find all required functions in the V-REP library. Cannot start 'RemoteApi' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
int vrepVer;
simGetIntegerParameter(sim_intparam_program_version,&vrepVer);
if (vrepVer<30200) // if V-REP version is smaller than 3.02.00
{
std::cout << "Sorry, your V-REP copy is somewhat old. Cannot start 'RemoteApi' plugin.\n";
unloadVrepLibrary(vrepLib);
return(0); // Means error, V-REP will unload this plugin
}
std::vector<int> inArgs;
// Register the new Lua commands:
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_START,inArgs);
simRegisterCustomLuaFunction(LUA_START_COMMAND,strConCat("number result=",LUA_START_COMMAND,"(number socketPort,number maxPacketSize=1300,boolean debug=false,boolean preEnableTrigger=false)"),&inArgs[0],LUA_START_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_STOP,inArgs);
simRegisterCustomLuaFunction(LUA_STOP_COMMAND,strConCat("number result=",LUA_STOP_COMMAND,"(number socketPort)"),&inArgs[0],LUA_STOP_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_RESET,inArgs);
simRegisterCustomLuaFunction(LUA_RESET_COMMAND,strConCat("number result=",LUA_RESET_COMMAND,"(number socketPort)"),&inArgs[0],LUA_RESET_CALLBACK);
CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_STATUS,inArgs);
simRegisterCustomLuaFunction(LUA_STATUS_COMMAND,strConCat("number status,table_5 info,number version,number clientVersion,string connectedIp=",LUA_STATUS_COMMAND,"(number socketPort)"),&inArgs[0],LUA_STATUS_CALLBACK);
// Read the configuration file to prepare socket connections:
CConfReader conf;
temp=currentDirAndPath;
#ifdef _WIN32
temp+="\\remoteApiConnections.txt";
#endif /* _WIN32 */
#if defined (__linux) || defined (__APPLE__)
temp+="/remoteApiConnections.txt";
#endif /* __linux || __APPLE__ */
// Read the configuration file and start remote API server services accordingly:
conf.readConfiguration(temp.c_str());
conf.getBoolean("useAlternateSocketRoutines",CSimxSocket::useAlternateSocketRoutines);
int index=1;
while (true)
{
std::stringstream strStream;
strStream << index;
std::string variableNameBase("portIndex");
variableNameBase+=strStream.str();
std::string variableName=variableNameBase+"_port";
int portNb;
if (conf.getInteger(variableName.c_str(),portNb))
{
bool debug=false;
int maxPacketSize=1300;
if (portNb<0)
maxPacketSize=3200000;
bool synchronousTrigger=false;
variableName=variableNameBase+"_maxPacketSize";
conf.getInteger(variableName.c_str(),maxPacketSize);
variableName=variableNameBase+"_debug";
conf.getBoolean(variableName.c_str(),debug);
variableName=variableNameBase+"_syncSimTrigger";
conf.getBoolean(variableName.c_str(),synchronousTrigger);
if (portNb<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;
}
if (allConnections.getConnectionFromPort(portNb)==NULL)
{
CSimxSocket* oneSocketConnection=new CSimxSocket(portNb,true,false,debug,maxPacketSize,synchronousTrigger);
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
std::cout << "Starting a remote API server on port " << portNb << std::endl;
}
else
std::cout << "Failed starting a remote API server on port " << portNb << std::endl;
index++;
}
else
break;
}
// Check if we need to start additional remote API server services:
for (int iarg=0;iarg<9;iarg++)
{
char* str=simGetStringParameter(sim_stringparam_app_arg1+iarg);
if (str!=NULL)
{
std::string arg(str);
simReleaseBuffer(str);
if ( (arg.compare(0,23,"REMOTEAPISERVERSERVICE_")==0)&&(arg.length()>23) )
{
size_t pos1=arg.find('_',24);
size_t pos2=std::string::npos;
size_t pos3=std::string::npos;
if (pos1!=std::string::npos)
pos2=arg.find('_',pos1+1);
if (pos2!=std::string::npos)
pos3=arg.find('_',pos2+1);
int portNb=-1;
bool debug=false;
bool syncTrigger=true;
int maxPacketSize=1300;
std::string portStr;
if (pos1!=std::string::npos)
{
portStr=arg.substr(23,pos1-23);
if (pos2!=std::string::npos)
{
debug=(arg.compare(pos1+1,pos2-pos1-1,"TRUE")==0);
if (pos3!=std::string::npos)
syncTrigger=(arg.compare(pos2+1,pos3-pos2-1,"TRUE")==0);
else
syncTrigger=(arg.compare(pos2+1,std::string::npos,"TRUE")==0);
}
else
debug=(arg.compare(pos1+1,std::string::npos,"TRUE")==0);
}
else
portStr=arg.substr(23,std::string::npos);
if (portStr.length()!=0)
{
portNb=atoi(portStr.c_str());
if (allConnections.getConnectionFromPort(portNb)==NULL)
{
CSimxSocket* oneSocketConnection=new CSimxSocket(portNb,true,false,debug,maxPacketSize,syncTrigger);
oneSocketConnection->start();
allConnections.addSocketConnection(oneSocketConnection);
std::cout << "Starting a remote API server on port " << portNb << std::endl;
}
else
std::cout << "Failed starting a remote API server on port " << portNb << std::endl;
}
}
}
}
return(SIMX_VERSION); // initialization went fine, we return the version number of this extension module (can be queried with simGetModuleName)
}
// This is the plugin messaging routine (i.e. V-REP calls this function very often, with various messages):
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// Keep following 5 lines at the beginning and unchanged:
//static bool refreshDlgFlag=true;
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
// Here we can intercept many messages from V-REP (actually callbacks). Only the most important messages are listed here.
// For a complete list of messages that you can intercept/react with, search for "sim_message_eventcallback"-type constants
// in the V-REP user manual.
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // The main script is about to be run (only called while a simulation is running (and not paused!))
// main script is called every dynamics calculation.
tickRTCs(simGetSimulationTimeStep());
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
simulatorClock.setSimulationTimeStep(simGetSimulationTimeStep());
//simulatorClock.setSimulationTime(simGetSimulationTime());
startRTCs();
}
if (message == sim_message_eventcallback_menuitemselected) {
simInt handle = auxiliaryData[0];
simInt state = auxiliaryData[1];
if (handle == robotItemHandle) {
std::cout << "Robot Menu Selected" << std::endl;
mainItemHandle = robotItemHandle;
spawnRTCMethod = spawnRobotRTC;
modelDlgHandle = simDisplayDialog("Input Robot Item", "Input Robot Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == rangeItemHandle) {
std::cout << "Range Menu Selected" << std::endl;
mainItemHandle = rangeItemHandle;
spawnRTCMethod = spawnRangeRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Range Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == cameraItemHandle) {
std::cout << "Camera Menu Selected" << std::endl;
mainItemHandle = cameraItemHandle;
spawnRTCMethod = spawnCameraRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Vision Sensor Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == accelItemHandle) {
std::cout << "Acceleration Menu Selected" << std::endl;
mainItemHandle = accelItemHandle;
spawnRTCMethod = spawnAccelerometerRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Accel Sensor Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == gyroItemHandle) {
std::cout << "Gyro Menu Selected" << std::endl;
mainItemHandle = gyroItemHandle;
spawnRTCMethod = spawnGyroRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Gyro Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == depthItemHandle) {
std::cout << "Depth Menu Selected" << std::endl;
mainItemHandle = depthItemHandle;
spawnRTCMethod = spawnDepthRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Depth Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
else if (handle == objectItemHandle) {
std::cout << "Object Menu Selected" << std::endl;
mainItemHandle = objectItemHandle;
spawnRTCMethod = spawnObjectRTC;
modelDlgHandle = simDisplayDialog("Input Item", "Input Object Model Name", sim_dlgstyle_input, "", NULL, NULL, NULL);
}
}
if (message == sim_message_eventcallback_refreshdialogs) {
if (modelDlgHandle >= 0) {
simInt ret = simGetDialogResult(modelDlgHandle);
if (ret != sim_dlgret_still_open) {
if (ret == sim_dlgret_ok) {
char *buf = simGetDialogInput(modelDlgHandle);
modelName = buf;
std::cout << "Model Name is " << buf << std::endl;
simReleaseBuffer(buf);
argDlgHandle = simDisplayDialog("Input Argument of RTC", "Input Argument of createComponent method", sim_dlgstyle_input, "", NULL, NULL, NULL);
modelDlgHandle = -1;
}
}
} else if (argDlgHandle >= 0) {
simInt ret = simGetDialogResult(argDlgHandle);
if (ret != sim_dlgret_still_open) {
if (ret == sim_dlgret_ok) {
char *buf = simGetDialogInput(argDlgHandle);
argument = buf;
std::cout << "createComponent Argument is " << buf << std::endl;
simReleaseBuffer(buf);
argDlgHandle = -1;
spawnRTCMethod(modelName, argument);
}
}
}
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
stopRTCs();
}
simulatorClock.setSimulationTime(simGetSimulationTime());
message_pump();
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
void CLuaFunctionData::releaseBuffers_luaFunctionCall(SLuaCallBack* p)
{ // use this you finished with a Lua function call from a plugin
simReleaseBuffer((char*)p->inputBool);
p->inputBool=NULL;
simReleaseBuffer((char*)p->inputInt);
p->inputInt=NULL;
simReleaseBuffer((char*)p->inputFloat);
p->inputFloat=NULL;
simReleaseBuffer((char*)p->inputDouble);
p->inputDouble=NULL;
simReleaseBuffer((char*)p->inputChar);
p->inputChar=NULL;
simReleaseBuffer((char*)p->inputCharBuff);
p->inputCharBuff=NULL;
simReleaseBuffer((char*)p->inputArgTypeAndSize);
p->inputArgTypeAndSize=NULL;
simReleaseBuffer((char*)p->outputBool);
p->outputBool=NULL;
simReleaseBuffer((char*)p->outputInt);
p->outputInt=NULL;
simReleaseBuffer((char*)p->outputFloat);
p->outputFloat=NULL;
simReleaseBuffer((char*)p->outputDouble);
p->outputDouble=NULL;
simReleaseBuffer((char*)p->outputChar);
p->outputChar=NULL;
simReleaseBuffer((char*)p->outputCharBuff);
p->outputCharBuff=NULL;
simReleaseBuffer((char*)p->outputArgTypeAndSize);
p->outputArgTypeAndSize=NULL;
}
void CK3::_updateStateVisualization()
{
_initialize();
// IR sensors on the base:
float ptAndDist[4];
std::string s;
for (int i=0;i<9;i++)
{
if (((simGetExplicitHandling(_k3IrSensorHandles[i])&1)==0)&&(simReadProximitySensor(_k3IrSensorHandles[i],ptAndDist,NULL,NULL)>0))
{
simSetUIButtonLabel(_k3DisplayHandle,110+i,"&&Box",NULL);
std::stringstream out;
out << int(ptAndDist[3]*1000.0f);
s=out.str();
simSetUIButtonLabel(_k3DisplayHandle,210+i,s.c_str(),NULL);
}
else
{
simSetUIButtonLabel(_k3DisplayHandle,110+i,"",NULL);
simSetUIButtonLabel(_k3DisplayHandle,210+i,"",NULL);
}
}
// UI title:
char* objName=simGetObjectName(_associatedObjectID);
if (objName!=NULL)
{
std::string nm(objName);
simReleaseBuffer(objName);
nm+=" state visualization";
simSetUIButtonLabel(_k3DisplayHandle,0,nm.c_str(),NULL);
}
// US sensors on the base:
for (int i=0;i<5;i++)
{
if (((simGetExplicitHandling(_k3UsSensorHandles[i])&1)==0)&&(simReadProximitySensor(_k3UsSensorHandles[i],ptAndDist,NULL,NULL)>0))
{
simSetUIButtonLabel(_k3DisplayHandle,100+i,"&&Box",NULL);
std::stringstream out;
out << int(ptAndDist[3]*1000.0f);
s=out.str();
simSetUIButtonLabel(_k3DisplayHandle,200+i,s.c_str(),NULL);
}
else
{
simSetUIButtonLabel(_k3DisplayHandle,100+i,"",NULL);
simSetUIButtonLabel(_k3DisplayHandle,200+i,"",NULL);
}
}
// Color sensors on base:
for (int i=0;i<2;i++)
{
float* auxValues=NULL;
int* auxValuesCount=NULL;
float col[3]={0.0f,0.0f,0.0f};
if (simReadVisionSensor(_k3ColorSensorHandles[i],&auxValues,&auxValuesCount)>=0)
{
if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
{
col[0]=auxValues[11];
col[1]=auxValues[12];
col[2]=auxValues[13];
}
simReleaseBuffer((char*)auxValues);
simReleaseBuffer((char*)auxValuesCount);
}
simSetUIButtonColor(_k3DisplayHandle,300+i,col,NULL,NULL);
}
// Base motor velocities and encoders:
for (int i=0;i<2;i++)
{
std::stringstream out1,out2;
out1 << std::fixed << std::setprecision(1) << _currentVelocities[i]*180.0f/piValue;
s=out1.str();
simSetUIButtonLabel(_k3DisplayHandle,320+i,s.c_str(),NULL);
out2 << int(float(ENCODER_IMPULSES_PER_TURN)*_cumulativeMotorAngles[i]/(2.0f*piValue));
s=out2.str();
simSetUIButtonLabel(_k3DisplayHandle,310+i,s.c_str(),NULL);
}
// Arm position:
std::stringstream out;
out << int((1.0f-(_currentArmPosition*180.0f/(195.0f*piValue)))*600.0f+300.0f);
s=out.str();
simSetUIButtonLabel(_k3DisplayHandle,120,s.c_str(),NULL);
simSetUISlider(_k3DisplayHandle,121,int(1000.0f*_currentArmPosition*180.0f/(195.0f*piValue)));
// Finger motors:
out.str("");
out << int(_currentGripperGap_unscaled*1000.0f);
s=out.str();
simSetUIButtonLabel(_k3DisplayHandle,130,s.c_str(),NULL);
simSetUISlider(_k3DisplayHandle,131,int(0.5f+(1.0f-_currentGripperGap_unscaled/0.055f)*1000.0f));
simSetUISlider(_k3DisplayHandle,132,int(0.5f+(_currentGripperGap_unscaled/0.055f)*1000.0f));
// Gripper proximity sensors:
for (int i=0;i<2;i++)
{
if (((simGetExplicitHandling(_k3GripperDistanceSensorHandles[i])&1)==0)&&(simReadProximitySensor(_k3GripperDistanceSensorHandles[i],ptAndDist,NULL,NULL)>0))
{
out.str("");
out << int(ptAndDist[3]*1000.0f);
s=out.str();
simSetUIButtonLabel(_k3DisplayHandle,133+i,s.c_str(),NULL);
}
else
simSetUIButtonLabel(_k3DisplayHandle,133+i,"",NULL);
}
// Gripper color sensors:
for (int i=0;i<2;i++)
{
float* auxValues=NULL;
int* auxValuesCount=NULL;
float col[3]={0.0f,0.0f,0.0f};
if (simReadVisionSensor(_k3GripperColorSensorHandles[i],&auxValues,&auxValuesCount)>=0)
{
if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
{
col[0]=auxValues[11];
col[1]=auxValues[12];
col[2]=auxValues[13];
}
simReleaseBuffer((char*)auxValues);
simReleaseBuffer((char*)auxValuesCount);
}
simSetUIButtonColor(_k3DisplayHandle,135+i,col,NULL,NULL);
}
}
void LaserScanPluglet::v_repMessage_callback() {
if (simGetSimulationState() != sim_simulation_advancing_running) {
return;
}
int packNumRead;
int packNumWrite;
simTubeStatus(tubeHandle, &packNumRead, &packNumWrite);
if (packNumRead > 0) {
int dlength = 0;
float* packet = (float*)simTubeRead(tubeHandle, &dlength);
float pos[3];
int size = dlength / sizeof(float);
int scan_size = size / 3;
sensor_msgs::LaserScan scan;
scan.header.frame_id = frame_id;
scan.header.stamp = ros::Time::now();
scan.angle_min = -240 * M_PI / 180 / 2;
scan.angle_max = -scan.angle_min;
scan.angle_increment = 2 * M_PI / 1024;
scan.range_min = 0.05;
scan.range_max = 4.5;
scan.scan_time = 0.05;
scan.intensities.resize(scan_size, 0.0);
scan.ranges.resize(scan_size, 0.0);
int scancounter = 0;
//simFloat matrix[12];
//simGetObjectMatrix(publishers[pubI].auxInt1, -1, matrix);
//simInvertMatrix(matrix);
for (int i=0; i<size-3; i+=3) {
pos[0] = packet[i+0];
pos[1] = packet[i+1];
pos[2] = packet[i+2];
//simTransformVector(matrix, pos);
//std::cout << "Tubedata received: " << pos[0] << ", " << pos[1] << ", " << pos[2] << ", " << std::endl;
float d = sqrt(pos[0]*pos[0] + pos[1]*pos[1]);
scan.ranges[scancounter++] = d;
}
publisher.publish(scan);
simReleaseBuffer((char*)packet);
} else {
//std::cout << "No Tubedata received" << std::endl;
}
}
VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customData,int* replyData)
{ // This is called quite often. Just watch out for messages/events you want to handle
// This function should not generate any error messages:
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
void* retVal=NULL;
if (message==sim_message_eventcallback_modulehandle)
{
if ( (customData==NULL)||(std::string("K3").compare((char*)customData)==0) ) // is the command also meant for Khepera3?
{
float dt=simGetSimulationTimeStep();
for (unsigned int k3Index=0;k3Index<allK3s.size();k3Index++)
{
// 1. Run the robot control:
for (int i=0;i<2;i++)
{
if (allK3s[k3Index].targetVelocities[i]>allK3s[k3Index].currentVelocities[i])
{
allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]+allK3s[k3Index].maxAcceleration*dt;
if (allK3s[k3Index].currentVelocities[i]>allK3s[k3Index].targetVelocities[i])
allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i];
}
else
{
allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].currentVelocities[i]-allK3s[k3Index].maxAcceleration*dt;
if (allK3s[k3Index].currentVelocities[i]<allK3s[k3Index].targetVelocities[i])
allK3s[k3Index].currentVelocities[i]=allK3s[k3Index].targetVelocities[i];
}
simSetJointTargetVelocity(allK3s[k3Index].wheelMotorHandles[i],allK3s[k3Index].currentVelocities[i]);
float jp;
simGetJointPosition(allK3s[k3Index].wheelMotorHandles[i],&jp);
float dp=jp-allK3s[k3Index].previousMotorAngles[i];
if (fabs(dp)>3.1415f)
dp-=(2.0f*3.1415f*fabs(dp)/dp);
allK3s[k3Index].cumulativeMotorAngles[i]+=dp; // corrected on 5/3/2012 thanks to Felix Fischer
allK3s[k3Index].previousMotorAngles[i]=jp;
}
float adp=allK3s[k3Index].targetArmPosition-allK3s[k3Index].currentArmPosition;
if (adp!=0.0f)
{
if (adp*allK3s[k3Index].currentArmVelocity>=0.0f)
{
float decelToZeroTime=(fabs(allK3s[k3Index].currentArmVelocity)+allK3s[k3Index].maxArmAcceleration*dt*1.0f)/allK3s[k3Index].maxArmAcceleration;
float distToZero=0.5f*allK3s[k3Index].maxArmAcceleration*decelToZeroTime*decelToZeroTime;
float dir=1.0f;
if (allK3s[k3Index].currentArmVelocity!=0.0f)
dir=allK3s[k3Index].currentArmVelocity/fabs(allK3s[k3Index].currentArmVelocity);
else
dir=adp/fabs(adp);
if (fabs(adp)>distToZero)
allK3s[k3Index].currentArmVelocity+=dir*allK3s[k3Index].maxArmAcceleration*dt;
else
allK3s[k3Index].currentArmVelocity-=dir*allK3s[k3Index].maxArmAcceleration*dt;
}
else
allK3s[k3Index].currentArmVelocity*=(1-allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity));
}
else
{
if (allK3s[k3Index].currentArmVelocity!=0.0f)
{
float dv=-allK3s[k3Index].currentArmVelocity*allK3s[k3Index].maxArmAcceleration*dt/fabs(allK3s[k3Index].currentArmVelocity);
if ((allK3s[k3Index].currentArmVelocity+dv)*allK3s[k3Index].currentArmVelocity<0.0f)
allK3s[k3Index].currentArmVelocity=0.0f;
else
allK3s[k3Index].currentArmVelocity+=dv;
}
}
allK3s[k3Index].currentArmPosition+=allK3s[k3Index].currentArmVelocity*dt;
simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[0],allK3s[k3Index].currentArmPosition);
simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[1],-allK3s[k3Index].currentArmPosition);
simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[2],allK3s[k3Index].currentArmPosition);
simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[3],allK3s[k3Index].currentArmPosition);
simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[4],allK3s[k3Index].currentArmPosition);
simSetJointTargetPosition(allK3s[k3Index].armMotorHandles[5],allK3s[k3Index].currentArmPosition);
float jp;
simGetJointPosition(allK3s[k3Index].fingerMotorHandles[0],&jp);
allK3s[k3Index].currentGripperGap=(jp)+0.04f;
float dp=allK3s[k3Index].targetGripperGap-allK3s[k3Index].currentGripperGap;
float velToRegulate=0.0f;
if (fabs(dp)<0.005f)
{
if (dp!=0.0f)
velToRegulate=(fabs(dp)/0.005f)*0.045f*dp/fabs(dp);
}
else
velToRegulate=0.045f*dp/fabs(dp);
simSetJointTargetVelocity(allK3s[k3Index].fingerMotorHandles[0],velToRegulate);
simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[1],-jp*0.5f);
simSetJointTargetPosition(allK3s[k3Index].fingerMotorHandles[2],-jp*0.5f);
// 2. Update the robot's UI:
// IR sensors on the base:
float ptAndDist[4];
std::string s;
for (int i=0;i<9;i++)
{
if (((simGetExplicitHandling(allK3s[k3Index].irSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].irSensorHandles[i],ptAndDist,NULL,NULL)>0))
{
simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"&&Box",NULL);
std::stringstream out;
out << int(ptAndDist[3]*1000.0f);
s=out.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,s.c_str(),NULL);
}
else
{
simSetUIButtonLabel(allK3s[k3Index].uiHandle,110+i,"",NULL);
simSetUIButtonLabel(allK3s[k3Index].uiHandle,210+i,"",NULL);
}
}
// UI title:
char* objName=simGetObjectName(allK3s[k3Index].k3BaseHandle);
if (objName!=NULL)
{
std::string nm(objName);
simReleaseBuffer(objName);
nm+=" state visualization";
simSetUIButtonLabel(allK3s[k3Index].uiHandle,0,nm.c_str(),NULL);
}
// US sensors on the base:
for (int i=0;i<5;i++)
{
if (((simGetExplicitHandling(allK3s[k3Index].usSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].usSensorHandles[i],ptAndDist,NULL,NULL)>0))
{
simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"&&Box",NULL);
std::stringstream out;
out << int(ptAndDist[3]*1000.0f);
s=out.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,s.c_str(),NULL);
}
else
{
simSetUIButtonLabel(allK3s[k3Index].uiHandle,100+i,"",NULL);
simSetUIButtonLabel(allK3s[k3Index].uiHandle,200+i,"",NULL);
}
}
// Color sensors on base:
for (int i=0;i<2;i++)
{
float* auxValues=NULL;
int* auxValuesCount=NULL;
float col[3]={0.0f,0.0f,0.0f};
if (simReadVisionSensor(allK3s[k3Index].colorSensorHandles[i],&auxValues,&auxValuesCount)>=0)
{
if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
{
col[0]=auxValues[11];
col[1]=auxValues[12];
col[2]=auxValues[13];
}
simReleaseBuffer((char*)auxValues);
simReleaseBuffer((char*)auxValuesCount);
}
simSetUIButtonColor(allK3s[k3Index].uiHandle,300+i,col,NULL,NULL);
}
// Base motor velocities and encoders:
for (int i=0;i<2;i++)
{
std::stringstream out1,out2;
out1 << std::fixed << std::setprecision(1) << allK3s[k3Index].currentVelocities[i]*180.0f/3.1415f;
s=out1.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,320+i,s.c_str(),NULL);
out2 << int(float(2764)*allK3s[k3Index].cumulativeMotorAngles[i]/(2.0f*3.1415f));
s=out2.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,310+i,s.c_str(),NULL);
}
// Arm position:
std::stringstream out;
out << int((1.0f-(allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f)))*600.0f+300.0f);
s=out.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,120,s.c_str(),NULL);
simSetUISlider(allK3s[k3Index].uiHandle,121,int(1000.0f*allK3s[k3Index].currentArmPosition*180.0f/(195.0f*3.1415f)));
// Finger motors:
out.str("");
out << int(allK3s[k3Index].currentGripperGap*1000.0f);
s=out.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,130,s.c_str(),NULL);
simSetUISlider(allK3s[k3Index].uiHandle,131,int(0.5f+(1.0f-allK3s[k3Index].currentGripperGap/0.055f)*1000.0f));
simSetUISlider(allK3s[k3Index].uiHandle,132,int(0.5f+(allK3s[k3Index].currentGripperGap/0.055f)*1000.0f));
// Gripper proximity sensors:
for (int i=0;i<2;i++)
{
if (((simGetExplicitHandling(allK3s[k3Index].gripperDistanceSensorHandles[i])&1)==0)&&(simReadProximitySensor(allK3s[k3Index].gripperDistanceSensorHandles[i],ptAndDist,NULL,NULL)>0))
{
out.str("");
out << int(ptAndDist[3]*1000.0f);
s=out.str();
simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,s.c_str(),NULL);
}
else
simSetUIButtonLabel(allK3s[k3Index].uiHandle,133+i,"",NULL);
}
// Gripper color sensors:
for (int i=0;i<2;i++)
{
float* auxValues=NULL;
int* auxValuesCount=NULL;
float col[3]={0.0f,0.0f,0.0f};
if (simReadVisionSensor(allK3s[k3Index].gripperColorSensorHandles[i],&auxValues,&auxValuesCount)>=0)
{
if ((auxValuesCount[0]>0)||(auxValuesCount[1]>=15))
{
col[0]=auxValues[11];
col[1]=auxValues[12];
col[2]=auxValues[13];
}
simReleaseBuffer((char*)auxValues);
simReleaseBuffer((char*)auxValuesCount);
}
simSetUIButtonColor(allK3s[k3Index].uiHandle,135+i,col,NULL,NULL);
}
}
}
}
if (message==sim_message_eventcallback_simulationended)
{ // simulation ended. Destroy all Khepera3 instances:
allK3s.clear();
}
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
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);
}
void CK3::_initialize()
{
if (_initialized)
return;
checkScale();
// We need to find the handle of K3's motors and sensors according to data tags attached to objects.
// Since there might be several K3s in the scene, we should only explore this K3's tree hierarchy
// to find those tags:
for (int i=0;i<2;i++)
_k3MotorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3ColorSensorHandles[i]=-1;
for (int i=0;i<9;i++)
_k3IrSensorHandles[i]=-1;
for (int i=0;i<5;i++)
_k3UsSensorHandles[i]=-1;
for (int i=0;i<6;i++)
_k3GripperArmMotorHandles[i]=-1;
for (int i=0;i<3;i++)
_k3GripperFingerMotorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3GripperDistanceSensorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3GripperColorSensorHandles[i]=-1;
std::vector<int> toExplore;
toExplore.push_back(_associatedObjectID); // We start exploration with the base of the K3-tree
while (toExplore.size()!=0)
{
int objHandle=toExplore[toExplore.size()-1];
toExplore.pop_back();
// 1. Add this object's children to the list to explore:
int index=0;
int childHandle=simGetObjectChild(objHandle,index++);
while (childHandle!=-1)
{
toExplore.push_back(childHandle);
childHandle=simGetObjectChild(objHandle,index++);
}
// 2. Now check if this object has one of the tags we are looking for:
// a. Get all the developer data attached to this scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objHandle,DEVELOPER_DATA_HEADER);
if (buffSize!=0)
{ // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
char* datBuff=new char[buffSize];
simGetObjectCustomData(objHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the searched tags:
std::vector<unsigned char> k3TagData;
if (CAccess::extractSerializationData(developerCustomData,K3_LEFTMOTOR,k3TagData))
_k3MotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_RIGHTMOTOR,k3TagData))
_k3MotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORLEFT,k3TagData))
_k3ColorSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORRIGHT,k3TagData))
_k3ColorSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR1,k3TagData))
_k3IrSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR2,k3TagData))
_k3IrSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR3,k3TagData))
_k3IrSensorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR4,k3TagData))
_k3IrSensorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR5,k3TagData))
_k3IrSensorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR6,k3TagData))
_k3IrSensorHandles[5]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR7,k3TagData))
_k3IrSensorHandles[6]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR8,k3TagData))
_k3IrSensorHandles[7]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR9,k3TagData))
_k3IrSensorHandles[8]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR1,k3TagData))
_k3UsSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR2,k3TagData))
_k3UsSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR3,k3TagData))
_k3UsSensorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR4,k3TagData))
_k3UsSensorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR5,k3TagData))
_k3UsSensorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR1,k3TagData))
_k3GripperArmMotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR2,k3TagData))
_k3GripperArmMotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR3,k3TagData))
_k3GripperArmMotorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR4,k3TagData))
_k3GripperArmMotorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR5,k3TagData))
_k3GripperArmMotorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR6,k3TagData))
_k3GripperArmMotorHandles[5]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR1,k3TagData))
_k3GripperFingerMotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR2,k3TagData))
_k3GripperFingerMotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR3,k3TagData))
_k3GripperFingerMotorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTDISTSENSOR,k3TagData))
_k3GripperDistanceSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTDISTSENSOR,k3TagData))
_k3GripperDistanceSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTCOLSENSOR,k3TagData))
_k3GripperColorSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTCOLSENSOR,k3TagData))
_k3GripperColorSensorHandles[1]=objHandle;
}
}
// Now get the display handle (Custom UI):
char* baseName=simGetObjectName(_associatedObjectID);
int suffixNumber=simGetNameSuffix(baseName);
simReleaseBuffer(baseName);
int globalSuffixSave=simGetNameSuffix(NULL);
simSetNameSuffix(suffixNumber);
_k3DisplayHandle=simGetUIHandle("K3_stateVisualization");
simSetNameSuffix(globalSuffixSave); // reset to previous value
_targetVelocities[0]=0.0f;
_targetVelocities[1]=0.0f;
_currentVelocities[0]=0.0f;
_currentVelocities[1]=0.0f;
_cumulativeMotorAngles[0]=0.0f;
_cumulativeMotorAngles[1]=0.0f;
_previousMotorAngles[0]=0.0f;
_previousMotorAngles[1]=0.0f;
_currentArmPosition=0.0f;
_targetArmPosition=0.0f;
_currentArmVelocity=0.0f;
_targetGripperGap_unscaled=0.055f;
_initialized=true;
}
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);
}
