void LUA_WAKEPUBLISHER_CALLBACK(SScriptCallBack* p)
{
CScriptFunctionData D;
int result=-1;
if (D.readDataFromStack(p->stackID,inArgs_WAKEPUBLISHER,inArgs_WAKEPUBLISHER[0],LUA_WAKEPUBLISHER_COMMAND))
{
std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int publishCnt=inData->at(1).int32Data[0];
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
result=ROS_server::wakePublisher(topicName.c_str(),publishCnt);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (result==-2)
simSetLastError(LUA_WAKEPUBLISHER_COMMAND, "Topic could not be found."); // output an error
}
else
simSetLastError(LUA_WAKEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
}
D.pushOutData(CScriptFunctionDataItem(result));
D.writeDataToStack(p->stackID);
}
void LUA_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);
}
int
event_queue_connect(struct event_queue *eq, struct queue_thread *me)
{
OutputPlugin *output;
std::string topicName(eq->dest_name);
if(eq->plugin_data == NULL) {
output = new OutputPlugin();
eq->plugin_data = (void*)output;
} else {
output = (OutputPlugin*)eq->plugin_data;
}
assert(output != NULL);
try {
glite_common_log(IL_LOG_CATEGORY, LOG_PRIORITY_DEBUG,
" trying to connect to %s",
eq->dest_name);
output->connect(topicName);
} catch(cms::CMSException &e) {
set_error(IL_DL, 0, e.what());
me->timeout = TIMEOUT;
return 0;
}
me->first_event_sent = 0;
eq->last_connected= time(NULL);
return 1;
}
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_ENABLEPUBLISHER_CALLBACK(SScriptCallBack* p)
{
CScriptFunctionData D;
std::string effectiveTopicName;
if (D.readDataFromStack(p->stackID,inArgs_ENABLEPUBLISHER,inArgs_ENABLEPUBLISHER[0]-1,LUA_ENABLEPUBLISHER_COMMAND)) // -1 because the last argument is optional
{
std::vector<CScriptFunctionDataItem>* inData=D.getInDataPtr();
std::string topicName(inData->at(0).stringData[0]);
if (topicName.length()>0)
{
int queueSize=inData->at(1).int32Data[0];
int streamCmd=inData->at(2).int32Data[0];
int auxInt1=inData->at(3).int32Data[0];
int auxInt2=inData->at(4).int32Data[0];
std::string auxString(inData->at(5).stringData[0]);
int publishCnt=0; // 0 is the default value for this optional argument
if (inData->size()>6)
publishCnt=inData->at(6).int32Data[0];
int errorModeSaved;
simGetIntegerParameter(sim_intparam_error_report_mode,&errorModeSaved);
simSetIntegerParameter(sim_intparam_error_report_mode,sim_api_errormessage_ignore);
effectiveTopicName=ROS_server::addPublisher(topicName.c_str(),queueSize,streamCmd,auxInt1,auxInt2,auxString.c_str(),publishCnt);
simSetIntegerParameter(sim_intparam_error_report_mode,errorModeSaved); // restore previous settings
if (effectiveTopicName.length()==0)
simSetLastError(LUA_ENABLEPUBLISHER_COMMAND, "Topic could not be published."); // output an error
}
else
simSetLastError(LUA_ENABLEPUBLISHER_COMMAND, "Invalid topic name."); // output an error
}
if (effectiveTopicName.size()!=0)
D.pushOutData(CScriptFunctionDataItem(effectiveTopicName));
D.writeDataToStack(p->stackID);
}
void DrawMeshHandler::_initialize() {
if (_initialized)
return;
std::vector<unsigned char> developerCustomData;
getDeveloperCustomData(developerCustomData);
std::vector<unsigned char> tempMainData;
std::stringstream ss;
if (CAccess::extractSerializationData(developerCustomData, DATA_MAIN,
tempMainData)) {
_frequency = CAccess::pop_float(tempMainData);
ss << "- [" << _associatedObjectName << "] Drawing frequency set to "
<< _frequency << "." << std::endl;
} else {
_frequency = -1;
ss << "- [" << _associatedObjectName
<< "] Drawing frequency not specified. Using simulation step as default."
<< std::endl;
}
if (CAccess::extractSerializationData(developerCustomData, DATA_WIDTH,
tempMainData)) {
_width = CAccess::pop_int(tempMainData);
ss << "- [" << _associatedObjectName << "] Line width set to " << _width
<< "." << std::endl;
} else {
_width = 1;
ss << "- [" << _associatedObjectName
<< "] Line width not specified. Using " << _width << " as default"
<< std::endl;
}
if (CAccess::extractSerializationData(developerCustomData, DATA_DIFFUSE,
tempMainData)) {
const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData, 3);
if (tmp.size() == 3) {
memcpy(_diffuse, tmp.data(), 3 * sizeof(simFloat));
ss << "- [" << _associatedObjectName << "] Diffuse color set to ["
<< _diffuse[0] << ", " << _diffuse[1] << ", " << _diffuse[2]
<< "]." << std::endl;
}
} else {
ss << "- [" << _associatedObjectName
<< "] Diffuse color not specified. Using [" << _diffuse[0]
<< ", " << _diffuse[1] << ", " << _diffuse[2] << "] as default"
<< std::endl;
}
if (CAccess::extractSerializationData(developerCustomData, DATA_SPECULAR,
tempMainData)) {
const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData, 3);
if (tmp.size() == 3) {
memcpy(_specular, tmp.data(), 3 * sizeof(simFloat));
ss << "- [" << _associatedObjectName << "] Specular color set to ["
<< _specular[0] << ", " << _specular[1] << ", "
<< _specular[2] << "]." << std::endl;
}
} else {
ss << "- [" << _associatedObjectName
<< "] Specular color not specified. Using [" << _specular[0]
<< ", " << _specular[1] << ", " << _specular[2]
<< "] as default" << std::endl;
}
if (CAccess::extractSerializationData(developerCustomData, DATA_EMISSION,
tempMainData)) {
const std::vector<simFloat> tmp = CAccess::pop_float(tempMainData, 3);
if (tmp.size() == 3) {
memcpy(_emission, tmp.data(), 3 * sizeof(simFloat));
ss << "- [" << _associatedObjectName << "] Emission color set to ["
<< _emission[0] << ", " << _emission[1] << ", "
<< _emission[2] << "]." << std::endl;
}
} else {
ss << "- [" << _associatedObjectName
<< "] Emission color not specified. Using [" << _emission[0]
<< ", " << _emission[1] << ", " << _emission[2]
<< "] as default" << std::endl;
}
if (CAccess::extractSerializationData(developerCustomData,
DATA_TRANSPARENCY, tempMainData)) {
_transparency = CAccess::pop_int(tempMainData);
switch (_transparency) {
case 0:
_transparency = 0;
ss << "- [" << _associatedObjectName
<< "] Transparency set to 100%." << std::endl;
break;
case 1:
_transparency = sim_drawing_50percenttransparency;
ss << "- [" << _associatedObjectName << "] Transparency set to 50%."
<< std::endl;
break;
case 2:
_transparency = sim_drawing_25percenttransparency;
ss << "- [" << _associatedObjectName << "] Transparency set to 25%."
<< std::endl;
break;
case 3:
_transparency = sim_drawing_12percenttransparency;
ss << "- [" << _associatedObjectName
<< "] Transparency set to 12.5%." << std::endl;
break;
default:
ss << "- [" << _associatedObjectName
<< "] The specified transparency value (" << _transparency
<< ") is not valid. Transparency value can only be 0 (100%), 1 (50%), 2 (25%), or 3 (12.5). Using 100% as default."
<< std::endl;
_transparency = 0;
}
} else {
_transparency = 0;
ss << "- [" << _associatedObjectName
<< "] Transparency not specified. Using 100% as default"
<< std::endl;
}
std::string topicName(_associatedObjectName);
std::replace(topicName.begin(), topicName.end(), '#', '_');
topicName += "/DrawMesh";
_sub = _nh.subscribe(topicName, 1, &DrawMeshHandler::msgCallback, this);
ss << "- [" << _associatedObjectName
<< "] Waiting for mesh triangles on topic " << topicName << "."
<< std::endl;
;
ConsoleHandler::printInConsole(ss);
_lastTime = -1e5;
_initialized = true;
}
// Main code:
int main(int argc,char* argv[])
{
// The joint handles and proximity sensor handles are given in the argument list
// (when V-REP launches this executable, V-REP will also provide the argument list)
int leftMotorHandle;
int rightMotorHandle;
int sensorHandle;
if (argc>=4)
{
leftMotorHandle=atoi(argv[1]);
rightMotorHandle=atoi(argv[2]);
sensorHandle=atoi(argv[3]);
}
else
{
printf("Indicate following arguments: 'leftMotorHandle rightMotorHandle sensorHandle'!\n");
sleep(5000);
return 0;
}
// Create a ROS node. The name has a random component:
int _argc = 0;
char** _argv = NULL;
struct timeval tv;
unsigned int timeVal=0;
if (gettimeofday(&tv,NULL)==0)
timeVal=(tv.tv_sec*1000+tv.tv_usec/1000)&0x00ffffff;
std::string nodeName("rosBubbleRob");
std::string randId(boost::lexical_cast<std::string>(timeVal+int(999999.0f*(rand()/(float)RAND_MAX))));
nodeName+=randId;
ros::init(_argc,_argv,nodeName.c_str());
if(!ros::master::check())
return(0);
ros::NodeHandle node("~");
printf("rosBubbleRob just started with node name %s\n",nodeName.c_str());
// 1. Let's subscribe to V-REP's info stream (that stream is the only one enabled by default,
// and the only one that can run while no simulation is running):
ros::Subscriber subInfo=node.subscribe("/vrep/info",1,infoCallback);
// 2. Request V-REP to launch a publisher for the BubbleRob's proximity sensor:
ros::ServiceClient client_enablePublisher=node.serviceClient<vrep_common::simRosEnablePublisher>("/vrep/simRosEnablePublisher");
vrep_common::simRosEnablePublisher srv_enablePublisher;
srv_enablePublisher.request.topicName="proxData"+randId; // the requested topic name
srv_enablePublisher.request.queueSize=1; // the requested publisher queue size (on V-REP side)
srv_enablePublisher.request.streamCmd=simros_strmcmd_read_proximity_sensor; // the requested publisher type
srv_enablePublisher.request.auxInt1=sensorHandle; // some additional information the publisher needs (what proximity sensor)
if ( client_enablePublisher.call(srv_enablePublisher)&&(srv_enablePublisher.response.effectiveTopicName.length()!=0) )
{ // ok, the service call was ok, and the publisher was succesfully started on V-REP side
// V-REP is now streaming the proximity sensor data!
// 3. Let's subscribe to that proximity sensor data:
std::string topicName("/vrep/");
topicName+=srv_enablePublisher.response.effectiveTopicName; // Make sure to use the returned topic name, not the requested one (can be same)
ros::Subscriber sub=node.subscribe(topicName.c_str(),1,sensorCallback);
// 4. Let's tell V-REP to subscribe to the motor speed topic (publisher to that topic will be created further down):
ros::ServiceClient client_enableSubscriber=node.serviceClient<vrep_common::simRosEnableSubscriber>("/vrep/simRosEnableSubscriber");
vrep_common::simRosEnableSubscriber srv_enableSubscriber;
srv_enableSubscriber.request.topicName="/"+nodeName+"/wheels"; // the topic name
srv_enableSubscriber.request.queueSize=1; // the subscriber queue size (on V-REP side)
srv_enableSubscriber.request.streamCmd=simros_strmcmd_set_joint_state; // the subscriber type
if ( client_enableSubscriber.call(srv_enableSubscriber)&&(srv_enableSubscriber.response.subscriberID!=-1) )
{ // ok, the service call was ok, and the subscriber was succesfully started on V-REP side
// V-REP is now listening to the desired motor joint states
// 5. Let's prepare a publisher of those motor speeds:
ros::Publisher motorSpeedPub=node.advertise<vrep_common::JointSetStateData>("wheels",1);
// 6. Finally we have the control loop:
float driveBackStartTime=-99.0f;
while (ros::ok()&&simulationRunning)
{ // this is the control loop (very simple, just as an example)
vrep_common::JointSetStateData motorSpeeds;
float desiredLeftMotorSpeed;
float desiredRightMotorSpeed;
if (simulationTime-driveBackStartTime<3.0f)
{ // driving backwards while slightly turning:
desiredLeftMotorSpeed=-3.1415*0.5;
desiredRightMotorSpeed=-3.1415*0.25;
}
else
{ // going forward:
desiredLeftMotorSpeed=3.1415;
desiredRightMotorSpeed=3.1415;
if (sensorTrigger)
driveBackStartTime=simulationTime; // We detected something, and start the backward mode
sensorTrigger=false;
}
// publish the motor speeds:
motorSpeeds.handles.data.push_back(leftMotorHandle);
motorSpeeds.handles.data.push_back(rightMotorHandle);
motorSpeeds.setModes.data.push_back(2); // 2 is the speed mode
motorSpeeds.setModes.data.push_back(2);
motorSpeeds.values.data.push_back(desiredLeftMotorSpeed);
motorSpeeds.values.data.push_back(desiredRightMotorSpeed);
motorSpeedPub.publish(motorSpeeds);
// handle ROS messages:
ros::spinOnce();
// sleep a bit:
usleep(5000);
}
}
}
ros::shutdown();
printf("rosBubbleRob just ended!\n");
return(0);
}