// 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
// 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_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
allConnections.instancePass();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // Main script is about to be run
if (allConnections.thereWasARequestToCallTheMainScript())
replyData[0]=0; // this tells V-REP that we don't wanna execute the main script
else
allConnections.mainScriptWillBeCalled(); // this simply tells all remote API server services to reactivate their triggers (if that function is enabled)
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
allConnections.simulationEnded();
}
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
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_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);
}
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
simLockInterface(1);
// 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;
// Acknowledgment message display:
// *****************************************************************
static int auxConsoleHandleForAcknowledgmentDisplay=-1;
static int acknowledgmentDisplayStartTime=0;
if (displayAcknowledgment)
{
auxConsoleHandleForAcknowledgmentDisplay=simAuxiliaryConsoleOpen("Acknowledgments",10,2+4+16,NULL,NULL,NULL,NULL);
simAuxiliaryConsolePrint(auxConsoleHandleForAcknowledgmentDisplay,"The wiimote plugin is courtesy of Eric Rohmer.\n\nThe wiimote plugin contains WiiYourself! wiimote code by gl.tter\nhttp://gl.tter.org");
acknowledgmentDisplayStartTime=timeGetTime();
displayAcknowledgment=false;
}
if ( (auxConsoleHandleForAcknowledgmentDisplay!=-1)&&(timeGetTime()-acknowledgmentDisplayStartTime>5000) )
{
simAuxiliaryConsoleClose(auxConsoleHandleForAcknowledgmentDisplay);
auxConsoleHandleForAcknowledgmentDisplay=-1;
}
// *****************************************************************
// Clean-up at simulation end:
// *****************************************************************
if (message==sim_message_eventcallback_simulationended)
{
if (auxConsoleHandleForAcknowledgmentDisplay!=-1)
{
simAuxiliaryConsoleClose(auxConsoleHandleForAcknowledgmentDisplay);
auxConsoleHandleForAcknowledgmentDisplay=-1;
}
for (int i=0;i<4;i++) // for the 4 devices
{
while (startCountPerDevice[i]>0)
{
startCountOverall--;
startCountPerDevice[i]--;
if (startCountOverall==0)
killThread();
}
}
}
// *****************************************************************
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
simLockInterface(0);
return(retVal);
}
// 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 6 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).
// 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_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
if (auxiliaryData[0]==urdfDialog->dialogMenuItemHandle)
urdfDialog->makeVisible(!urdfDialog->getVisible()); // Toggle visibility of the dialog
}
if (message==sim_message_eventcallback_instancepass)
{ // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
urdfDialog->handleCommands();
urdfDialog->setSimulationStopped(simGetSimulationState()==sim_simulation_stopped);
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{
refreshDlgFlag=true;
}
}
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialog:
urdfDialog->refresh(); // Refresh the dialog
refreshDlgFlag=false;
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
// 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 4 lines at the beginning and unchanged:
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:
if (message==sim_message_eventcallback_instancepass)
{
// This message is sent each time the scene was rendered (well, shortly after) (very often)
// When a simulation is not running, but you still need to execute some commands, then put some code here
ROS_server::instancePass();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{
// Main script is about to be run (only called while a simulation is running (and not paused!))
//
// This is a good location to execute simulation commands
if (ROS_server::mainScriptAboutToBeCalled())
{
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,1); // the remote API client can query that value
replyData[0]=0; // this tells V-REP that we don't wanna execute the main script
}
else
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,0); // the remote API client can query that value
}
if (message==sim_message_eventcallback_simulationabouttostart)
{
// Simulation is about to start
ROS_server::simulationAboutToStart();
}
if (message==sim_message_eventcallback_simulationended)
{
// Simulation just ended
simSetBooleanParameter(sim_boolparam_waiting_for_trigger,0); // the remote API client can query that value
ROS_server::simulationEnded();
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
void ROS_server::spinOnce()
{
// Disable error reporting (it is enabled in the service processing part, but we don't want error reporting for publishers/subscribers)
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
//Handle all streaming (publishers)
streamAllData();
//Process all requested services and topic subscriptions
ros::spinOnce();
// Restore previous error report mode:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved);
}
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!
}
}
// 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:
simLockInterface(1);
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:
if (message==sim_message_eventcallback_simulationabouttostart)
{
// Simulation is about to start
ROS_INFO("[V-REP_DXL_ARM] Simulation is about to start.");
getJointHandles();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{
std::cout << " ---- ---- ---- " << std::endl;
// A simulation iteration is about to take place
for (int i = 0; i < JOINT_COUNT; ++i)
{
simGetJointPosition(joint_handles[i], &msg.data[i]);
msg.data[i] *= RAD_TO_DEG;
msg.data[i] += 150;
std::cout << msg.data[i] << std::endl;
}
msg.data[JOINT_COUNT] = 150; // Grasp -> center position
pub.publish(msg);
}
if (message==sim_message_eventcallback_simulationended)
{
// Simulation just ended
ROS_INFO("[V-REP_DXL_ARM] Simulation just ended.");
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings
simLockInterface(0);
return (retVal);
}
void CSubscriberData::setIntegerParameterCallback(const std_msgs::Int32::ConstPtr& param)
{
if (_handleGeneralCallback_before())
{
if (simSetIntegerParameter(auxInt1,param->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
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_simulationabouttostart)
simulationAboutToStart();
if (message==sim_message_eventcallback_simulationended)
simulationEnded();
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
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_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_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);
}
// 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 6 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).
// 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_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
if (auxiliaryData[0]==colladaDialog->dialogMenuItemHandle)
{
colladaDialog->makeVisible(!colladaDialog->getVisible()); // Toggle visibility of the dialog
}
}
if (message==sim_message_eventcallback_colladaplugin)
{
if (auxiliaryData[0]==0)
replyData[0]=PLUGIN_VERSION; // this is the version number of this plugin
if (auxiliaryData[0]==1)
replyData[0]=colladaDialog->importSingleGroupedShape((const char*)customData,(auxiliaryData[1]&1)!=0,float(auxiliaryData[2])/1000.0f);
}
if (message==sim_message_eventcallback_instancepass)
{ // It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
colladaDialog->handleCommands();
colladaDialog->setSimulationStopped(simGetSimulationState()==sim_simulation_stopped);
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{
refreshDlgFlag=true;
}
}
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialog:
colladaDialog->refresh(); // Refresh the dialog
refreshDlgFlag=false;
}
if (message==sim_message_eventcallback_simulationabouttostart)
{
colladaDialog->simulationAboutToStart();
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{
colladaDialog->mainScriptAboutToBeCalled();
}
if (message==sim_message_eventcallback_beforerendering)
{ // we are still in the main SIM thread!
colladaDialog->renderingPassAboutToBeCalled();
}
if (message==sim_message_eventcallback_simulationended)
{
colladaDialog->simulationEnded();
}
// You can add many more messages to handle here
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
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
}
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);
}
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
WIN_AFX_MANAGE_STATE;
simLockInterface(1);
static bool refreshDlgFlag=true;
// 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_refreshdialogs){
// V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
refreshDlgFlag=true;
}
if (message==sim_message_eventcallback_menuitemselected){
// A custom menu bar entry was selected..
}
if (message==sim_message_eventcallback_instancepass){
// It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
const int flags=auxiliaryData[0];
const bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
const bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{ // we actualize plugin objects for changes in the scene
objectContainer->actualizeForSceneContent();
refreshDlgFlag=true;
}
}
if (message==sim_message_eventcallback_moduleopen)
{ // A script called simOpenModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp(pluginName,(char*)customData)==0) ) // is the command also meant for this plugin?
objectContainer->startOfSimulation(); // yes!
}
if (message==sim_message_eventcallback_modulehandle){
// A script called simHandleModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp(pluginName,(char*)customData)==0) ) // is the command also meant for this plugin?
{
//clock_t init, final;
//init=clock();
objectContainer->handleSimulation();
//final=clock()-init;
//std::cout << (double)final / ((double)CLOCKS_PER_SEC) << std::endl;
}// yes!
}
if (message==sim_message_eventcallback_moduleclose){
// A script called simCloseModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp(pluginName,(char*)customData)==0) ) // is the command also meant for this plugin?
objectContainer->endOfSimulation(); // yes!
}
if (message==sim_message_eventcallback_instanceswitch){
// Here the user switched the instance. React to this message in a similar way as you would react to a full
// scene content change. In this plugin example, we react to an instance switch by reacting to the
// sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
// (see here above)
}
if (message==sim_message_eventcallback_broadcast){
// Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
}
if (message==sim_message_eventcallback_scenesave){
// The scene is about to be saved. If required do some processing here (e.g. add custom scene data to be serialized with the scene)
}
if (message==sim_message_eventcallback_simulationabouttostart){
// Simulation is about to start
}
if (message==sim_message_eventcallback_simulationended){
// Simulation just ended
}
// You can add more messages to handle here
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag){
// handle refresh of the plugin's dialog:
refreshDlgFlag=false;
}
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
simLockInterface(0);
return(retVal);
}
// 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_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
// here you could make a plugin's main dialog visible/invisible
}
if (message==sim_message_eventcallback_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
// It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{ // we actualize plugin objects for changes in the scene
//...
refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
}
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // The main script is about to be run (only called while a simulation is running (and not paused!))
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
}
if (message==sim_message_eventcallback_moduleopen)
{ // A script called simOpenModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the beginning of a simulation
}
}
if (message==sim_message_eventcallback_modulehandle)
{ // A script called simHandleModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only while a simulation is running
}
}
if (message==sim_message_eventcallback_moduleclose)
{ // A script called simCloseModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the end of a simulation
}
}
if (message==sim_message_eventcallback_instanceswitch)
{ // We switched to a different scene. Such a switch can only happen while simulation is not running
}
if (message==sim_message_eventcallback_broadcast)
{ // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
}
if (message==sim_message_eventcallback_scenesave)
{ // The scene is about to be saved. If required do some processing here (e.g. add custom scene data to be serialized with the scene)
}
// You can add many more messages to handle here
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialogs
// ...
refreshDlgFlag=false;
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
// 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:
simLockInterface(1);
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:
if (message==sim_message_eventcallback_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
// When a simulation is not running, but you still need to execute some commands, then put some code here
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // Main script is about to be run (only called while a simulation is running (and not paused!))
//
// This is a good location to execute commands (e.g. commands needed to generate ROS messages)
//
// e.g. to read all joint positions:
simLockInterface(1);
ros::spinOnce();
sensor_msgs::Image image_msg;
//
// Execute player speceific functions
//
if ((player_turn_state == PLAYER_NONE) && new_player_turn)
{
//ROS_INFO("New player turn received: %f %f", player_turn.power, player_turn.angle);
ROS_INFO("[PLAYER_NONE] Player turn ended.\n");
new_player_turn = false;
}
else if (player_turn_state == PLAYER_ROTATE_MOVE)
{
tip_position[0] = table_state.white_ball.x - TIP_OFFSET_DISTANCE * sin(player_turn.angle);
tip_position[1] = table_state.white_ball.y + TIP_OFFSET_DISTANCE * cos(player_turn.angle);
tip_orientation[2] = player_turn.angle;
std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Orientation success: " <<
simSetObjectOrientation(tip_handle, WORLD_FRAME, tip_orientation) << std::endl;
std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_ROTATE_MOVE] Position success: " <<
simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;
player_turn_state = PLAYER_CUE_DOWN;
}
else if (player_turn_state == PLAYER_CUE_DOWN)
{
tip_position[2] = CUE_ON_TABLE_HEIGHT;
std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_CUE_DOWN] Position success: " <<
simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;
player_turn_state = PLAYER_EXECUTE_SHOT;
}
else if (player_turn_state == PLAYER_EXECUTE_SHOT)
{
double dist = player_turn.power * POWER_DISTANCE_RATIO + MIN_POWER_DISTANCE;
tip_position[0] += sin(player_turn.angle) * dist;
tip_position[1] -= cos(player_turn.angle) * dist;
std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_EXECUTE_SHOT] Position success: " <<
simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;
player_turn_state = PLAYER_RAISE_CUE;
}
else if (player_turn_state == PLAYER_RAISE_CUE)
{
tip_position[2] = CUE_RAISED_HEIGHT;
std::cout << "[V-REP_ROS_POOL_SIM][PLAYER_RAISE_CUE] Position success: " <<
simSetObjectPosition(tip_handle, WORLD_FRAME, tip_position) << std::endl;
player_turn_state = PLAYER_NONE;
}
generate_image_message(image_msg);
//
// Transfer to old table state and get new state
//
old_table_state = table_state;
get_ball_positions();
get_cue_position_orientation();
//
//Check if table is in steady state
//
steady_state = (table_state.white_ball.z < POOL_TABLE_HEIGHT)?
(true):(closeTo(old_table_state.white_ball, table_state.white_ball, STEADY_STATE_DRIFT));
for (unsigned int i = 0; (i < BALL_COUNT) && steady_state; i++)
{
if (!closeTo(old_table_state.balls[i], table_state.balls[i], STEADY_STATE_DRIFT))
{
steady_state = false;
}
}
//
// Publish information
//
if (steady_state && (player_turn_state == PLAYER_NONE && new_player_turn == false))
{
ROS_INFO("Table is in steady state!\n");
//calculate turn score and publish it.
int score = calculate_turn_score();
ROS_INFO("The score for the last turn is %d", score);
std_msgs::Int32 score_msg;
score_msg.data = score;
score_pub.publish(score_msg);
usleep(50 * 1000); //50ms
table_state_pub.publish(table_state); // publish a message only if nothing is moving
}
//
// Move white ball if potted
//
if (steady_state && (table_state.white_ball.z < POOL_TABLE_HEIGHT)&&
(player_turn_state == PLAYER_NONE && new_player_turn == false))
{
std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " ";
std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " <<
simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl;
table_state.white_ball.x = white_ball_initial_position[0];
table_state.white_ball.y = white_ball_initial_position[1];
table_state.white_ball.z = white_ball_initial_position[2];
}
//
// Move all red balls is all potted
//
if (steady_state)
{
int balls_in = 0;
for (unsigned int i = 0; i < BALL_COUNT; i++)
{
if (table_state.balls[i].z < POOL_TABLE_HEIGHT)
{
balls_in++;
}
}
if (balls_in == BALL_COUNT)
{
//Move all balls to the top!
std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(white_ball_handle) << " ";
std::cout << "[V-REP_ROS_POOL_SIM] Moving white to initial position success: " <<
simSetObjectPosition(white_ball_handle, WORLD_FRAME, white_ball_initial_position) << std::endl;
for (unsigned int i = 0; i < BALL_COUNT; i++)
{
std::cout << "[V-REP_ROS_POOL_SIM] Ball " << i << std::endl;
float p[3];
p[0] = initial_table_state.balls[i].x;
p[1] = initial_table_state.balls[i].y;
p[2] = initial_table_state.balls[i].z;
std::cout << "[V-REP_ROS_POOL_SIM] Reset: " << simResetDynamicObject(balls_handle[i]) << std::endl;
std::cout << "[V-REP_ROS_POOL_SIM] Moving ball position success: " <<
simSetObjectPosition(balls_handle[i], WORLD_FRAME, p) << std::endl;
}
table_state = initial_table_state;
games_played++;
ROS_INFO("The are %d games played so far.", games_played);
}
}
simLockInterface(0);
//Publish messages
img_pub.publish(image_msg);
// The best would be to start ROS activity when a simulation is running, and stop it when a simulation ended
// If you allow ROS activity while a simulation is not running, then it becomes a little bit tricky when a scene
// was switched for example (e.g. the clients would need a way to detect that)
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
// Here you could launch the ROS node (if using just one node)
// If more than one node should be supported (e.g. with different services per node),
// then it is better to start a node via a custom Lua function
white_ball_handle = simGetObjectHandle("white_ball");
std::cout << "[V-REP POOL PLUGIN] White ball handle: " << white_ball_handle << std::endl;
tip_handle = simGetObjectHandle("tip");
for (int i=0; i < BALL_COUNT; i++)
{
std::stringstream ball_name;
ball_name << "ball";
ball_name << i;
balls_handle[i] = simGetObjectHandle(ball_name.str().c_str());
//std::cout << "Ball " << i << " handle: " << balls_handle[i] << std::endl;
}
camera_handle = simGetObjectHandle("camera_sensor");
std::cout << "[V-REP POOL PLUGIN] Camera handle: " << camera_handle << std::endl;
//Read all positions and make them as old positions
get_ball_positions();
old_table_state = table_state;
pre_hit_table_state = table_state;
initial_table_state = table_state;
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
// Here you could kill the ROS node(s) that are still active. There could also be a custom Lua function to kill a specific ROS node.
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode, errorModeSaved); // restore previous settings
simLockInterface(0);
return (retVal);
}
// 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_refreshdialogs)
refreshDlgFlag=true; // V-REP dialogs were refreshed. Maybe a good idea to refresh this plugin's dialog too
if (message==sim_message_eventcallback_menuitemselected)
{ // A custom menu bar entry was selected..
// here you could make a plugin's main dialog visible/invisible
}
if (message==sim_message_eventcallback_instancepass)
{ // This message is sent each time the scene was rendered (well, shortly after) (very often)
// It is important to always correctly react to events in V-REP. This message is the most convenient way to do so:
int flags=auxiliaryData[0];
bool sceneContentChanged=((flags&(1+2+4+8+16+32+64+256))!=0); // object erased, created, model or scene loaded, und/redo called, instance switched, or object scaled since last sim_message_eventcallback_instancepass message
bool instanceSwitched=((flags&64)!=0);
if (instanceSwitched)
{
// React to an instance switch here!!
}
if (sceneContentChanged)
{ // we actualize plugin objects for changes in the scene
refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here
}
//...
//////////////
// PUT MAIN CODE HERE
/////////////
if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running
{
//if(kukaPluginPG) BOOST_LOG_TRIVIAL(info) << "current simulation time:" << simGetSimulationTime() << std::endl; // gets simulation time point
}
// make sure it is "right" (what does that mean?)
// find the v-rep C functions to do the following:
////////////////////////////////////////////////////
// Use handles that were found at the "start" of this simulation running
// next few Lines get the joint angles, torque, etc from the simulation
if (kukaPluginPG)// && kukaPluginPG->allHandlesSet == true // allHandlesSet now handled internally
{
try
{
// run one loop synchronizing the arm and plugin
kukaPluginPG->run_one();
} 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();
}
}
}
if (message==sim_message_eventcallback_mainscriptabouttobecalled)
{ // The main script is about to be run (only called while a simulation is running (and not paused!))
}
if (message==sim_message_eventcallback_simulationabouttostart)
{ // Simulation is about to start
/////////////////////////
// PUT OBJECT STARTUP CODE HERE
////////////////////
// get the handles to all the objects, joints, etc that we need
/////////////////////
// simGetObjectHandle
// try {
// BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n";
// kukaPluginPG = std::make_shared<grl::KukaVrepPlugin>();
// kukaPluginPG->construct();
// //kukaPluginPG->run_one(); // for debugging purposes only
// //kukaPluginPG.reset(); // for debugging purposes only
// } catch (boost::exception& e){
// // log the error and print it to the screen, don't release the exception
// std::string initerr("v_repExtKukaLBRiiwa plugin initialization error:\n" + boost::diagnostic_information(e));
// simAddStatusbarMessage( initerr.c_str());
// BOOST_LOG_TRIVIAL(error) << initerr;
// }
}
if (message==sim_message_eventcallback_simulationended)
{ // Simulation just ended
/////////////////////////
// PUT OBJECT RESET CODE HERE
// close out as necessary
////////////////////
BOOST_LOG_TRIVIAL(info) << "Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n";
kukaPluginPG.reset();
}
if (message==sim_message_eventcallback_moduleopen)
{ // A script called simOpenModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the beginning of a simulation
}
}
if (message==sim_message_eventcallback_modulehandle)
{ // A script called simHandleModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only while a simulation is running
}
}
if (message==sim_message_eventcallback_moduleclose)
{ // A script called simCloseModule (by default the main script). Is only called during simulation.
if ( (customData==NULL)||(_stricmp("PluginSkeleton",(char*)customData)==0) ) // is the command also meant for this plugin?
{
// we arrive here only at the end of a simulation
}
}
if (message==sim_message_eventcallback_instanceswitch)
{ // Here the user switched the scene. React to this message in a similar way as you would react to a full
// scene content change. In this plugin example, we react to an instance switch by reacting to the
// sim_message_eventcallback_instancepass message and checking the bit 6 (64) of the auxiliaryData[0]
// (see here above)
}
if (message==sim_message_eventcallback_broadcast)
{ // Here we have a plugin that is broadcasting data (the broadcaster will also receive this data!)
}
if (message==sim_message_eventcallback_scenesave)
{ // The scene is about to be saved. If required do some processing here (e.g. add custom scene data to be serialized with the scene)
}
// You can add many more messages to handle here
if ((message==sim_message_eventcallback_guipass)&&refreshDlgFlag)
{ // handle refresh of the plugin's dialogs
// ...
refreshDlgFlag=false;
}
// Keep following unchanged:
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
return(retVal);
}
// 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);
}
