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()); }