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