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::setStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
if (_handleGeneralCallback_before())
{
simSetStringSignal(auxStr.c_str(),&sig->data[0],sig->data.length());
_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::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::appendStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
std::string theNewString;
int stringLength;
char* stringSignal=simGetStringSignal(auxStr.c_str(),&stringLength);
if (stringSignal!=NULL)
{
theNewString=std::string(stringSignal,stringLength);
simReleaseBuffer(stringSignal);
}
theNewString+=std::string(&sig->data[0],sig->data.length());
simSetStringSignal(auxStr.c_str(),theNewString.c_str(),theNewString.length());
}
void CSubscriberData::setJoySensorCallback(const sensor_msgs::Joy::ConstPtr& joyPacket)
{
float floatPacket[auxInt1 + auxInt2];
if((auxInt1>0)&&(auxInt2>0))
{
for(unsigned int i=0;i<auxInt1;i++)
floatPacket[i] = joyPacket->axes[i];
for(unsigned int j=0;j<auxInt2;j++)
floatPacket[j+auxInt1] = (float)joyPacket->buttons[j];
simSetStringSignal(auxStr.c_str(),(simChar*)floatPacket,4*(auxInt1+auxInt2));
}
else
shutDownGeneralSubscriber();
}
void CSubscriberData::setStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
simSetStringSignal(auxStr.c_str(),&sig->data[0],sig->data.length());
}
