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());
}
// Publishers:
void ROS_server::streamAllData()
{
// Take care of publishers here (i.e. have them publish their data):
float val[6];
// std::string string_signal_name = imu_publisher.auxStr;
// std::string string_signal_name = imu_publisher.auxStr;
int signalLength;
const char* signalValue=simGetStringSignal("imu_signal",&signalLength);
if (signalValue!=0)
{
if (signalLength>=6*sizeof(float))
{
for (int i=0;i<6;i++)
val[i]=((float*)signalValue)[i];
}
}
sensor_msgs::Imu fl;
//fl.header.seq=_simulationFrameID;
fl.header.stamp=ros::Time::now();
fl.orientation.x=0.0; //TODO orientation
fl.orientation.y=0.0;
fl.orientation.z=0.0;
fl.orientation.w=1.0;
fl.angular_velocity.x=(double)val[0];
fl.angular_velocity.y=(double)val[1];
fl.angular_velocity.z=(double)val[2];
fl.linear_acceleration.x=(double)val[3];
fl.linear_acceleration.y=(double)val[4];
fl.linear_acceleration.z=(double)val[5];
imu_publisher.publish(fl);
}
