void CSubscriberData::setJoySensorCallback(const sensor_msgs::Joy::ConstPtr& joyPacket)
{
if (_handleGeneralCallback_before())
{
if((auxInt1>=0)&&(auxInt2>=0)&&(auxInt2+auxInt1>0))
{
float floatPacket[auxInt1 + auxInt2];
for(unsigned int i=0;i<(unsigned int)auxInt1;i++)
{
if (i<joyPacket->axes.size())
floatPacket[i] = joyPacket->axes[i];
else
floatPacket[i] = 0.0f;
}
for(unsigned int j=0;j<(unsigned int)auxInt2;j++)
{
if (j<joyPacket->buttons.size())
floatPacket[j+auxInt1] = (float)joyPacket->buttons[j];
else
floatPacket[j+auxInt1] = 0.0f;
}
simSetStringSignal(auxStr.c_str(),(simChar*)floatPacket,4*(auxInt1+auxInt2));
}
else
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setVisionSensorImageCallback(const sensor_msgs::Image::ConstPtr& image)
{
if (_handleGeneralCallback_before())
{
int resol[2];
int result=simGetVisionSensorResolution(auxInt1,resol);
if (result!=-1)
{
if ((image->encoding==sensor_msgs::image_encodings::RGB8)&&(image->step==image->width*3)&&(int(image->height)==resol[1])&&(int(image->width)==resol[0]))
{
float* image_buff=new float[3*resol[0]*resol[1]];
for(unsigned int i=0;i<image->height;i++)
{
int msg_idx=(image->height-i-1)*image->step;
int buf_idx=i*image->step;
for(unsigned int j=0;j<image->step;j++)
image_buff[buf_idx+j]=float(image->data[msg_idx+j])/255.0f;
}
simSetVisionSensorImage(auxInt1,image_buff);
delete[] image_buff;
}
}
else
shutDownImageSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setIntegerSignalCallback(const std_msgs::Int32::ConstPtr& sig)
{
if (_handleGeneralCallback_before())
{
simSetIntegerSignal(auxStr.c_str(),sig->data);
_handleGeneralCallback_after();
}
}
void CSubscriberData::setStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
if (_handleGeneralCallback_before())
{
simSetStringSignal(auxStr.c_str(),&sig->data[0],sig->data.length());
_handleGeneralCallback_after();
}
}
void CSubscriberData::setFloatingParameterCallback(const std_msgs::Float32::ConstPtr& param)
{
if (_handleGeneralCallback_before())
{
if (simSetFloatingParameter(auxInt1,param->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setObjectIntParameterCallback(const std_msgs::Int32::ConstPtr& param)
{
if (_handleGeneralCallback_before())
{
if (simSetObjectIntParameter(auxInt1,auxInt2,param->data)<=0)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setUIButtonPropertyCallback(const std_msgs::Int32::ConstPtr& prop)
{
if (_handleGeneralCallback_before())
{
if (simSetUIButtonProperty(auxInt1,auxInt2,prop->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setJointForceCallback(const std_msgs::Float64::ConstPtr& force)
{
if (_handleGeneralCallback_before())
{
if (simSetJointForce(auxInt1,(float)force->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setUISlider(const std_msgs::Int32::ConstPtr& pos)
{
if (_handleGeneralCallback_before())
{
if (simSetUISlider(auxInt1,auxInt2,pos->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::addStatusbarMessageCallback(const std_msgs::String::ConstPtr& msg)
{
if (_handleGeneralCallback_before())
{
if (simAddStatusbarMessage(msg->data.c_str())==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setJointTargetVelocityCallback(const std_msgs::Float64::ConstPtr& vel)
{
if (_handleGeneralCallback_before())
{
if (simSetJointTargetVelocity(auxInt1,(float)vel->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setJointTargetPositionCallback(const std_msgs::Float64::ConstPtr& pos)
{
if (_handleGeneralCallback_before())
{
if (simSetJointTargetPosition(auxInt1,(float)pos->data)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setUIButtonLabelCallback(const std_msgs::String::ConstPtr& label)
{
if (_handleGeneralCallback_before())
{
if (simSetUIButtonLabel(auxInt1,auxInt2,label->data.c_str(),NULL)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setObjectQuaternionCallback(const geometry_msgs::Quaternion::ConstPtr& quaternion)
{
if (_handleGeneralCallback_before())
{
float v[4]={(float)quaternion->x,(float)quaternion->y,(float)quaternion->z,(float)quaternion->w};
if (simSetObjectQuaternion(auxInt1,auxInt2,v)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setTwistCommandCallback(const geometry_msgs::Twist::ConstPtr& vel)
{
if (_handleGeneralCallback_before())
{
simFloat command[6] = {vel->linear.x,vel->linear.y,vel->linear.z,vel->angular.x,vel->angular.y,vel->angular.z};
if (simSetStringSignal(auxStr.c_str(),(simChar*)command,6*sizeof(simFloat))==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setObjectPositionCallback(const geometry_msgs::Point::ConstPtr& pos)
{
if (_handleGeneralCallback_before())
{
float v[3]={(float)pos->x,(float)pos->y,(float)pos->z};
if (simSetObjectPosition(auxInt1,auxInt2,v)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setTracksVelCommandCallback(const nifti_robot_driver_msgs::Tracks::ConstPtr& vel)
{
if (_handleGeneralCallback_before())
{
simFloat command[2] = {vel->left,vel->right};
if (simSetStringSignal(auxStr.c_str(),(simChar*)command,2*sizeof(simFloat))==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::setArrayParameterCallback(const geometry_msgs::Point32::ConstPtr& param)
{
if (_handleGeneralCallback_before())
{
float v[3]={param->x,param->y,param->z};
if (simSetArrayParameter(auxInt1,(void*)v)==-1)
shutDownGeneralSubscriber();
_handleGeneralCallback_after();
}
}
void CSubscriberData::clearStringSignalCallback(const std_msgs::UInt8::ConstPtr& options)
{
if (_handleGeneralCallback_before())
{
if (options->data==0)
simClearStringSignal(auxStr.c_str());
else
simClearStringSignal(NULL);
_handleGeneralCallback_after();
}
}
void CSubscriberData::setObjectSelectionCallback(const std_msgs::Int32MultiArray::ConstPtr& objHandles)
{
if (_handleGeneralCallback_before())
{
simRemoveObjectFromSelection(sim_handle_all,0);
int cnt=objHandles->data.size();
for (int i=0;i<cnt;i++)
simAddObjectToSelection(sim_handle_single,objHandles->data[i]);
_handleGeneralCallback_after();
}
}
void CSubscriberData::setObjectPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
if (_handleGeneralCallback_before())
{
float p[3]={(float)pose->pose.position.x,(float)pose->pose.position.y,(float)pose->pose.position.z};
float q[4]={(float)pose->pose.orientation.x,(float)pose->pose.orientation.y,(float)pose->pose.orientation.z,(float)pose->pose.orientation.w};
if (simSetObjectPosition(auxInt1,auxInt2,p)==-1)
shutDownGeneralSubscriber();
else
simSetObjectQuaternion(auxInt1,auxInt2,q);
_handleGeneralCallback_after();
}
}
void CSubscriberData::appendStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
if (_handleGeneralCallback_before())
{
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());
_handleGeneralCallback_after();
}
}
void CSubscriberData::auxiliaryConsolePrintCallback(const std_msgs::String::ConstPtr& txt)
{
if (_handleGeneralCallback_before())
{
if (txt->data.length()==0)
{
if (simAuxiliaryConsolePrint(auxInt1,NULL)<=0)
shutDownGeneralSubscriber();
}
else
{
if (simAuxiliaryConsolePrint(auxInt1,txt->data.c_str())<=0)
shutDownGeneralSubscriber();
}
_handleGeneralCallback_after();
}
}
void CSubscriberData::callScriptFunctionCallback(const vrep_common::ScriptFunctionCallData::ConstPtr& data)
{
if (_handleGeneralCallback_before())
{
int options=auxInt1;
int inStringCnt=0;
std::string inStrings(data->stringData.data.begin(),data->stringData.data.end());
if (inStrings.size()>0)
{
if (inStrings[inStrings.size()-1]!='\0')
inStrings+='\0';
for (size_t i=0;i<inStrings.size();i++)
{
if (inStrings[i]=='\0')
inStringCnt++;
}
}
std::vector<std::string> inString;
int off=0;
for (int i=0;i<inStringCnt;i++)
{
inString.push_back(inStrings.c_str()+off);
off+=strlen(inStrings.c_str()+off)+1;
}
int inBufferSize=data->bufferData.data.size();
CScriptFunctionData D;
D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(data->intData.data));
D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(data->floatData.data));
D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(inString));
D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(data->bufferData.data.c_str(),inBufferSize));
int stackHandle=simCreateStack();
D.writeDataToStack_scriptFunctionCall(stackHandle);
simCallScriptFunctionEx(options,auxStr.c_str(),stackHandle);
simReleaseStack(stackHandle);
_handleGeneralCallback_after();
}
}
void CSubscriberData::setJointStateCallback(const vrep_common::JointSetStateData::ConstPtr& data)
{
if (_handleGeneralCallback_before())
{
if ( (data->handles.data.size()>0)&&(data->handles.data.size()==data->setModes.data.size())&&(data->handles.data.size()==data->values.data.size()) )
{
for (unsigned int i=0;i<data->handles.data.size();i++)
{
int handle=data->handles.data[i];
unsigned char setMode=data->setModes.data[i];
float val=data->values.data[i];
if (setMode==0)
simSetJointPosition(handle,val);
if (setMode==1)
simSetJointTargetPosition(handle,val);
if (setMode==2)
simSetJointTargetVelocity(handle,val);
if (setMode==3)
simSetJointForce(handle,val);
}
}
_handleGeneralCallback_after();
}
}
