void robot::setLocalRespondableMaskCummulative_alternate(int objHandle,bool bitSet)
{
if (simGetObjectType(objHandle)==sim_object_shape_type)
{
int p;
simGetObjectIntParameter(objHandle,3004,&p);
if (p!=0)
{
if (bitSet)
simSetObjectIntParameter(objHandle,3019,0xff01);
else
simSetObjectIntParameter(objHandle,3019,0xff02);
bitSet=!bitSet;
}
}
int index=0;
while (true)
{
int childHandle=simGetObjectChild(objHandle,index++);
if (childHandle==-1)
break;
setLocalRespondableMaskCummulative_alternate(childHandle,bitSet);
}
}
CSubscriberData::CSubscriberData(ros::NodeHandle* node,const char* _topicName,int queueSize,int _streamCmd,int _auxInt1,int _auxInt2,const char* _auxString,int _callbackTag_before,int _callbackTag_after,image_transport::ImageTransport* images_streamer[1],int& imgStreamerCnt)
{
cmdID=_streamCmd;
auxInt1=_auxInt1;
auxInt2=_auxInt2;
auxStr=_auxString;
callbackTag_before=_callbackTag_before;
callbackTag_after=_callbackTag_after;
topicName=_topicName;
isValid=false;
if (cmdID==simros_strmcmd_add_status_bar_message)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::addStatusbarMessageCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_auxiliary_console_print)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::auxiliaryConsolePrintCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_clear_float_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::clearFloatSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_clear_integer_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::clearIntegerSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_clear_string_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::clearStringSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_array_parameter)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setArrayParameterCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_boolean_parameter)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setBooleanParameterCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_floating_parameter)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setFloatingParameterCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_integer_parameter)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setIntegerParameterCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_float_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setFloatSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_integer_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setIntegerSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_string_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setStringSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_append_string_signal)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::appendStringSignalCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_joint_force)
{
if (simGetObjectType(auxInt1)==sim_object_joint_type)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJointForceCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_joint_position)
{
if (simGetObjectType(auxInt1)==sim_object_joint_type)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJointPositionCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_joint_target_position)
{
if (simGetObjectType(auxInt1)==sim_object_joint_type)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJointTargetPositionCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_joint_target_velocity)
{
if (simGetObjectType(auxInt1)==sim_object_joint_type)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJointTargetVelocityCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_twist_command)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setTwistCommandCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_tracks_vel_command)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setTracksVelCommandCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_object_float_parameter)
{
if (simGetObjectType(auxInt1)!=-1)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setObjectFloatParameterCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_object_int_parameter)
{
if (simGetObjectType(auxInt1)!=-1)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setObjectIntParameterCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_object_pose)
{
if ( (simGetObjectType(auxInt1)!=-1)&&( (simGetObjectType(auxInt2)!=-1)||(auxInt2==sim_handle_parent)||(auxInt2==-1) ) )
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setObjectPoseCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_object_position)
{
if ( (simGetObjectType(auxInt1)!=-1)&&( (simGetObjectType(auxInt2)!=-1)||(auxInt2==sim_handle_parent)||(auxInt2==-1) ) )
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setObjectPositionCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_object_quaternion)
{
if ( (simGetObjectType(auxInt1)!=-1)&&( (simGetObjectType(auxInt2)!=-1)||(auxInt2==sim_handle_parent)||(auxInt2==-1) ) )
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setObjectQuaternionCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_object_selection)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setObjectSelectionCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_ui_button_label)
{
if (simGetUIButtonProperty(auxInt1,auxInt2)!=-1)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setUIButtonLabelCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_ui_button_property)
{
if (simGetUIButtonProperty(auxInt1,auxInt2)!=-1)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setUIButtonPropertyCallback,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_ui_slider)
{
if (simGetUIButtonProperty(auxInt1,auxInt2)!=-1)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setUISlider,this);
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_vision_sensor_image)
{
if (simGetObjectType(auxInt1)==sim_object_visionsensor_type)
{
if (imgStreamerCnt==0)
images_streamer[0]= new image_transport::ImageTransport(*node);
imageSubscriber=images_streamer[0]->subscribe(topicName,queueSize,&CSubscriberData::setVisionSensorImageCallback,this);
imgStreamerCnt++;
isValid=true;
}
}
if (cmdID==simros_strmcmd_set_joy_sensor)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJoySensorCallback,this);
isValid=true;
}
if (cmdID==simros_strmcmd_set_joint_state)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJointStateCallback,this);
isValid=true;
}
/*
if (cmdID==simros_strmcmd_set_joint_trajectory)
{
generalSubscriber=node->subscribe(topicName,queueSize,&CSubscriberData::setJointTrajectoryCallback,this);
isValid=true;
}
*/
}
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
}
