collision_space::EnvironmentModel::AllowedCollisionMatrix
planning_environment::convertFromACMMsgToACM(const arm_navigation_msgs::AllowedCollisionMatrix& matrix)
{
std::map<std::string, unsigned int> indices;
std::vector<std::vector<bool> > vecs;
unsigned int ns = matrix.link_names.size();
if(matrix.entries.size() != ns) {
ROS_WARN_STREAM("Matrix messed up");
}
vecs.resize(ns);
for(unsigned int i = 0; i < std::min(ns, (unsigned int) matrix.entries.size()); i++) {
indices[matrix.link_names[i]] = i;
if(matrix.entries[i].enabled.size() != ns) {
ROS_WARN_STREAM("Matrix messed up");
}
vecs[i].resize(ns);
for(unsigned int j = 0; j < std::min(ns, (unsigned int) matrix.entries[i].enabled.size()); j++) {
vecs[i][j] = matrix.entries[i].enabled[j];
}
}
collision_space::EnvironmentModel::AllowedCollisionMatrix acm(vecs,indices);
return acm;
}
Holder<SoundHandle> SDLAudio::Play(const char* ResRef, int XPos, int YPos, unsigned int flags, unsigned int *length)
{
// TODO: some panning
(void)XPos;
(void)YPos;
// TODO: flags
(void)flags;
if (!ResRef) {
if (flags & GEM_SND_SPEECH) {
Mix_HaltChannel(0);
}
return Holder<SoundHandle>();
}
// TODO: move this loading code somewhere central
ResourceHolder<SoundMgr> acm(ResRef);
if (!acm) {
print("failed acm load\n");
return Holder<SoundHandle>();
}
int cnt = acm->get_length();
int riff_chans = acm->get_channels();
int samplerate = acm->get_samplerate();
// Use 16-bit word for memory allocation because read_samples takes a 16 bit alignment
short * memory = (short *) malloc(cnt*2);
//multiply always with 2 because it is in 16 bits
int cnt1 = acm->read_samples( memory, cnt ) * 2;
//Sound Length in milliseconds
unsigned int time_length = ((cnt / riff_chans) * 1000) / samplerate;
if (length) {
*length = time_length;
}
// convert our buffer, if necessary
SDL_AudioCVT cvt;
SDL_BuildAudioCVT(&cvt, AUDIO_S16SYS, riff_chans, samplerate,
audio_format, audio_channels, audio_rate);
cvt.buf = (Uint8*)malloc(cnt1*cvt.len_mult);
memcpy(cvt.buf, (char*)memory, cnt1);
cvt.len = cnt1;
SDL_ConvertAudio(&cvt);
// free old buffer
free(memory);
// make SDL_mixer chunk
Mix_Chunk *chunk = Mix_QuickLoad_RAW(cvt.buf, cvt.len*cvt.len_ratio);
if (!chunk) {
print("error loading chunk\n");
return Holder<SoundHandle>();
}
// play
int channel = -1;
if (flags & GEM_SND_SPEECH) {
channel = 0;
}
SDL_mutexP(OurMutex);
channel = Mix_PlayChannel(channel, chunk, 0);
if (channel < 0) {
SDL_mutexV(OurMutex);
print("error playing channel\n");
return Holder<SoundHandle>();
}
assert((unsigned int)channel < channel_data.size());
channel_data[channel] = cvt.buf;
SDL_mutexV(OurMutex);
// TODO
return Holder<SoundHandle>();
}