void HeaderManipulation::inputCB(const topic_tools::ShapeShifter::ConstPtr &input)
{
ROS_DEBUG("HeaderManipulation inputCB");
ros::Time start_time(ros::Time::now());
// Initialize.
if (!output_advertised_) {
ROS_INFO("Output not advertised. Setting up publisher now.");
ros::AdvertiseOptions opts("output", 1, input->getMD5Sum(),
input->getDataType(), input->getMessageDefinition());
boost::mutex::scoped_lock pub_lock(pub_mutex_);
generic_pub_ = private_nh_.advertise(opts);
output_advertised_ = true;
}
// Copy shape shifter data to array.
uint8_t msg_buffer[input->size()];
ros::serialization::OStream o_stream(msg_buffer, input->size());
input->write(o_stream);
// Manipulate (header) data.
manipulateRawData(msg_buffer);
// Read data from array to StampedMsg
boost::shared_ptr<StampedMsg> stamped_msg(new StampedMsg());
stamped_msg->second = start_time;
ros::serialization::IStream i_stream(msg_buffer, input->size());
stamped_msg->first.read(i_stream);
// Push StampedMsg to Buffer for multithreaded publishing.
boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
stamped_msg_buffer_.push(stamped_msg);
ROS_DEBUG("all done");
}
/*
* Submit an I/O request on a buffer.
*/
int submit_block(int rw, struct buffer *buf)
{
struct block_device *dev = get_device(buf->b_dev);
struct request *req = make_request(rw, buf);
buffer_lock(buf);
if (request_queue_empty(dev))
dev->handle_request(dev, req);
else
list_add_tail(&req->chain, &dev->requests);
return 0;
}
void wbtl_file::swrite(void* buffer, offset_type offset, size_type bytes)
{
scoped_mutex_lock buffer_lock(buffer_mutex);
// is the block already mapped?
{
scoped_mutex_lock mapping_lock(mapping_mutex);
sortseq::iterator physical = address_mapping.find(offset);
STXXL_VERBOSE_WBTL("wbtl:swrite l" << FMT_A_S(offset, bytes) << " @ <= p" <<
FMT_A_C(physical != address_mapping.end() ? physical->second : 0xffffffff, address_mapping.size()));
if (physical != address_mapping.end()) {
mapping_lock.unlock();
// FIXME: special case if we can replace it in the current writing block
discard(offset, bytes);
}
}
if (bytes > write_block_size - curpos)
{
// not enough space in the current write buffer
if (buffer_address[curbuf] != offset_type(-1)) {
STXXL_VERBOSE_WBTL("wbtl:w2disk p" << FMT_A_S(buffer_address[curbuf], write_block_size));
// mark remaining part as free
if (curpos < write_block_size)
_add_free_region(buffer_address[curbuf] + curpos, write_block_size - curpos);
if (backend_request.get()) {
backend_request->wait(false);
}
backend_request = storage->awrite(write_buffer[curbuf], buffer_address[curbuf], write_block_size);
}
curbuf = 1 - curbuf;
buffer_address[curbuf] = get_next_write_block();
curpos = 0;
}
assert(bytes <= write_block_size - curpos);
// write block into buffer
memcpy(write_buffer[curbuf] + curpos, buffer, bytes);
stats::get_instance()->write_cached(bytes);
scoped_mutex_lock mapping_lock(mapping_mutex);
address_mapping[offset] = buffer_address[curbuf] + curpos;
reverse_mapping[buffer_address[curbuf] + curpos] = place(offset, bytes);
STXXL_VERBOSE_WBTL("wbtl:swrite l" << FMT_A_S(offset, bytes) << " @ => p" << FMT_A_C(buffer_address[curbuf] + curpos, address_mapping.size()));
curpos += bytes;
}
void wbtl_file::sread(void* buffer, offset_type offset, size_type bytes)
{
scoped_mutex_lock buffer_lock(buffer_mutex);
int cached = -1;
offset_type physical_offset;
// map logical to physical address
{
scoped_mutex_lock mapping_lock(mapping_mutex);
sortseq::iterator physical = address_mapping.find(offset);
if (physical == address_mapping.end()) {
STXXL_ERRMSG("wbtl_read: mapping not found: " << FMT_A_S(offset, bytes) << " ==> " << "???");
//STXXL_THROW_ERRNO(io_error, "wbtl_read of unmapped memory");
physical_offset = 0xffffffff;
} else {
physical_offset = physical->second;
}
}
if (buffer_address[curbuf] <= physical_offset &&
physical_offset < buffer_address[curbuf] + write_block_size)
{
// block is in current write buffer
assert(physical_offset + bytes <= buffer_address[curbuf] + write_block_size);
memcpy(buffer, write_buffer[curbuf] + (physical_offset - buffer_address[curbuf]), bytes);
stats::get_instance()->read_cached(bytes);
cached = curbuf;
}
else if (buffer_address[1 - curbuf] <= physical_offset &&
physical_offset < buffer_address[1 - curbuf] + write_block_size)
{
// block is in previous write buffer
assert(physical_offset + bytes <= buffer_address[1 - curbuf] + write_block_size);
memcpy(buffer, write_buffer[1 - curbuf] + (physical_offset - buffer_address[1 - curbuf]), bytes);
stats::get_instance()->read_cached(bytes);
cached = curbuf;
}
else if (physical_offset == 0xffffffff) {
// block was deleted or never written before
char* uninitialized = (char*)malloc(sizeof(char));
memset(buffer, *uninitialized, bytes);
free(uninitialized);
}
else
{
// block is not cached
request_ptr req = storage->aread(buffer, physical_offset, bytes);
req->wait(false);
}
STXXL_VERBOSE_WBTL("wbtl:sread l" << FMT_A_S(offset, bytes) << " @ p" << FMT_A(physical_offset) << " " << std::dec << cached);
STXXL_UNUSED(cached);
}
// Handle a notify message
void lister_handle_notify(Lister *lister,DOpusNotify *notify,char *name)
{
BOOL show=0;
// Lock buffer
buffer_lock(lister->cur_buffer,TRUE);
// Create dir?
if (notify->dn_Flags&DNF_DOS_CREATEDIR)
{
DirEntry *entry;
BPTR lock;
struct FileInfoBlock __aligned fib;
// Lock directory
if (lock=Lock(name,ACCESS_READ))
{
// Examine it
Examine(lock,&fib);
UnLock(lock);
// Create entry
if (entry=create_file_entry(
lister->cur_buffer,0,
fib.fib_FileName,
fib.fib_Size,
fib.fib_DirEntryType,
&fib.fib_Date,
fib.fib_Comment,
fib.fib_Protection,
0,0,0,0))
{
// Add to buffer
add_file_entry(lister->cur_buffer,entry,0);
// Save date in buffer (assume this is the latest thing!)
lister->cur_buffer->buf_DirectoryDate=fib.fib_Date;
// Mark for refresh
show=1;
}
}
}
// Unlock buffer
buffer_unlock(lister->cur_buffer);
// Refresh?
if (show) lister_refresh_display(lister,REFRESHF_SLIDERS|REFRESHF_STATUS);
}
void HeaderManipulation::publishMsgLoop(const ros::NodeHandle &nh)
{
ROS_DEBUG("HeaderManipulation publishMsgLoop");
while (nh.ok())
{
{
boost::mutex::scoped_lock buffer_lock(buffer_mutex_);
if (!stamped_msg_buffer_.empty())
{
ROS_DEBUG("publishing msg");
publishMsg(stamped_msg_buffer_.front());
stamped_msg_buffer_.pop();
continue;
}
}
publish_retry_rate_.sleep();
}
}
/**
* Reallocate the video buffer if the video format or resolution changes
*/
void allocateBufferVideo(
ImageBuffer& buffer,
const freenect_video_format& format,
const freenect_resolution& resolution,
const freenect_registration& registration) {
// Obtain a lock on the buffer. This is mostly for debugging, as allocate
// buffer should only be called when the buffer is not being used by the
// freenect thread
boost::lock_guard<boost::mutex> buffer_lock(buffer.mutex);
// Deallocate the buffer incase an exception happens (the buffer should no
// longer be valid)
buffer.image_buffer.reset();
switch (format) {
case FREENECT_VIDEO_RGB:
case FREENECT_VIDEO_BAYER:
case FREENECT_VIDEO_YUV_RGB:
case FREENECT_VIDEO_IR_8BIT:
case FREENECT_VIDEO_IR_10BIT:
case FREENECT_VIDEO_IR_10BIT_PACKED:
switch (resolution) {
case FREENECT_RESOLUTION_HIGH:
case FREENECT_RESOLUTION_MEDIUM:
buffer.metadata =
freenect_find_video_mode(resolution, format);
if (!buffer.metadata.is_valid) {
throw std::runtime_error("libfreenect: Invalid video fmt, res: " +
boost::lexical_cast<std::string>(format) + "," +
boost::lexical_cast<std::string>(resolution));
}
break;
default:
throw std::runtime_error("libfreenect: Invalid video resolution: " +
boost::lexical_cast<std::string>(resolution));
}
break;
default:
throw std::runtime_error("libfreenect: Invalid video format: " +
boost::lexical_cast<std::string>(format));
}
// All is good, reallocate the buffer and calculate other pieces of info
buffer.image_buffer.reset(new unsigned char[buffer.metadata.bytes]);
switch(format) {
case FREENECT_VIDEO_RGB:
case FREENECT_VIDEO_BAYER:
case FREENECT_VIDEO_YUV_RGB:
buffer.focal_length = getRGBFocalLength(buffer.metadata.width);
break;
case FREENECT_VIDEO_IR_8BIT:
case FREENECT_VIDEO_IR_10BIT:
case FREENECT_VIDEO_IR_10BIT_PACKED:
buffer.focal_length = getDepthFocalLength(registration,
buffer.metadata.width);
break;
default:
throw std::runtime_error("libfreenect: shouldn't reach here");
}
buffer.is_registered = false;
}
// Searches for an empty buffer, or one with the same name (preferred)
// If a suitable buffer is not found, it uses the current buffer
// Called from the LISTER PROCESS
DirBuffer *lister_buffer_find_empty(Lister *lister,char *path,struct DateStamp *stamp)
{
DirBuffer *buffer,*first_empty=0,*first_unlocked=0;
#ifdef DEBUG
if (lister) check_call("lister_buffer_find_empty",lister);
#endif
// Check we're not showing special
check_special_buffer(lister,1);
// If current buffer is empty, use that one
if (!(lister->cur_buffer->flags&DWF_VALID))
{
// Lock buffer
buffer_lock(lister->cur_buffer,TRUE);
// Free buffer
buffer_freedir(lister->cur_buffer,1);
// Unlock it
buffer_unlock(lister->cur_buffer);
return lister->cur_buffer;
}
// Lock buffer list
lock_listlock(&GUI->buffer_list,TRUE);
// Go through buffers in this list (backwards)
for (buffer=(DirBuffer *)GUI->buffer_list.list.lh_TailPred;
buffer->node.ln_Pred;
buffer=(DirBuffer *)buffer->node.ln_Pred)
{
// See if this directory is available and matches our path
if (path && stricmp(buffer->buf_Path,path)==0 &&
(!stamp || CompareDates(&buffer->buf_VolumeDate,stamp)==0))
{
// Not locked, or in use by this lister?
if (!buffer->buf_CurrentLister || buffer->buf_CurrentLister==lister)
{
// Store pointer
first_empty=buffer;
break;
}
}
// If directory is empty, store pointer (if the first one)
if (!buffer->buf_CurrentLister && !(buffer->flags&DWF_VALID) && !first_empty)
first_empty=buffer;
// First unlocked buffer?
if (!buffer->buf_CurrentLister && !first_unlocked)
{
// Because buffers are moved to the head of the list whenever
// they are accessed, this will point to the unlocked buffer
// that was accessed the longest time ago
first_unlocked=buffer;
}
}
// If we found an empty one, use that one
if (!(buffer=first_empty))
{
// Allocate a new buffer if we're allowed to
if (GUI->buffer_count<environment->env->settings.max_buffer_count &&
!(environment->env->settings.dir_flags&DIRFLAGS_DISABLE_CACHING))
{
// If this fails, use first unlocked
buffer=lister_new_buffer(lister);
}
}
// If nothing yet, use first unlocked buffer (if there is one)
if (!buffer && !(buffer=first_unlocked))
{
// If not, re-use current buffer
buffer=lister->cur_buffer;
}
// Lock buffer
buffer_lock(buffer,TRUE);
// Free buffer
buffer_freedir(buffer,1);
// Unlock buffer
buffer_unlock(buffer);
// Show buffer in lister
lister_show_buffer(lister,buffer,0,1);
// Unlock buffer list
unlock_listlock(&GUI->buffer_list);
// Return current buffer
return lister->cur_buffer;
}
// Run a function on some icons
void icon_function(BackdropInfo *info,BackdropObject *only_one,char *data,Cfg_Function *func,ULONG flags)
{
short count=0;
struct ArgArray *array;
BackdropObject *object;
DirBuffer *buffer=0;
char *source_path=0;
// Lock backdrop list
lock_listlock(&info->objects,1);
// Go through backdrop list
for (object=(BackdropObject *)info->objects.list.lh_Head;
object->node.ln_Succ;
object=(BackdropObject *)object->node.ln_Succ)
{
// Skip invalid icons
if (object->type!=BDO_LEFT_OUT ||
(!info->lister && !(object->flags&BDOF_DESKTOP_FOLDER))) continue;
// Want this icon?
if (!only_one || only_one==object)
{
// Is object selected?
if ((only_one || object->state) && object->icon)
{
// Increment counts
if (object->icon->do_Type==WBDRAWER ||
object->icon->do_Type==WBGARBAGE ||
object->icon->do_Type==WBPROJECT ||
object->icon->do_Type==WBTOOL)
++count;
// Only doing one?
if (only_one) break;
}
}
}
// Nothing to work on?
if (count==0 ||
!(array=NewArgArray()))
{
unlock_listlock(&info->objects);
return;
}
// Got a lister?
if (info->lister)
{
// Lock buffer
buffer_lock((buffer=info->lister->cur_buffer),FALSE);
}
// Source path from icon?
else
if (only_one && only_one->path &&
(source_path=AllocVec(strlen(only_one->path)+1,0)))
strcpy(source_path,only_one->path);
// Otherwise, assume this is the desktop folder
else
if (info->flags&BDIF_MAIN_DESKTOP &&
(source_path=AllocVec(strlen(environment->env->desktop_location)+1,0)))
strcpy(source_path,environment->env->desktop_location);
// Go through backdrop list again
for (object=(BackdropObject *)info->objects.list.lh_Head;
object->node.ln_Succ;
object=(BackdropObject *)object->node.ln_Succ)
{
// Skip invalid icons
if (object->type!=BDO_LEFT_OUT ||
(!info->lister && !(object->flags&BDOF_DESKTOP_FOLDER))) continue;
// Want this icon?
if (!only_one || only_one==object)
{
// Is object selected?
if ((only_one || object->state) && object->icon)
{
char name[80];
BOOL dir=0,link=0,icon=0;
DirEntry *entry=0;
struct ArgArrayEntry *aae;
// Build name
stccpy(name,object->name,sizeof(name));
// Got a buffer?
if (buffer)
{
// See if we can find this entry
if ((entry=find_entry(&buffer->entry_list,object->name,0,buffer->more_flags&DWF_CASE)) ||
(entry=find_entry(&buffer->reject_list,object->name,0,buffer->more_flags&DWF_CASE)))
{
// Directory?
if (entry->de_Node.dn_Type>=ENTRY_DEVICE) dir=1;
// Link?
if (entry->de_Flags&ENTF_LINK) link=1;
// See if entry has icon
if (find_entry(&buffer->entry_list,object->name,0,(buffer->more_flags&DWF_CASE)|FINDENTRY_ICON) ||
find_entry(&buffer->reject_list,object->name,0,(buffer->more_flags&DWF_CASE)|FINDENTRY_ICON))
icon=1;
}
// If not, use the .info name
else
{
// Add .info
strcat(name,".info");
}
}
// Desktop folder?
else
if (info->flags&BDIF_MAIN_DESKTOP)
{
// Assume the file has an icon
icon=1;
}
// Get type from icon
if (!entry)
{
if (object->icon->do_Type==WBDRAWER ||
object->icon->do_Type==WBGARBAGE) dir=1;
if (object->flags&BDOF_LINK_ICON) link=1;
}
// Tack on a / for directories
if (dir) strcat(name,"/");
// Allocate array entry
if (aae=NewArgArrayEntry(array,name))
{
// Dir?
if (dir) aae->ae_Flags|=AEF_DIR;
// Link?
if (link) aae->ae_Flags|=AEF_LINK;
// No icon?
if (!icon) aae->ae_Flags|=AEF_FAKE_ICON;
}
// Only doing one?
if (only_one) break;
}
}
}
// Unlock buffer
if (buffer) buffer_unlock(buffer);
// Get flags we need
flags|=(data && *data)?FUNCF_ICONS:FUNCF_ICONS|FUNCF_ASK_DEST;
// Launch the function
function_launch(
FUNCTION_RUN_FUNCTION_EXTERNAL,
func,
0,
flags,
info->lister,0,
(info->lister)?info->lister->cur_buffer->buf_Path:source_path,data,
array,
0,0);
// Free source path
FreeVec(source_path);
// Unlock list
unlock_listlock(&info->objects);
}
gralloc1_error_t BufferManager::AllocateBuffers(uint32_t num_descriptors,
const gralloc1_buffer_descriptor_t *descriptor_ids,
buffer_handle_t *out_buffers) {
bool shared = true;
gralloc1_error_t status = GRALLOC1_ERROR_NONE;
// since GRALLOC1_CAPABILITY_TEST_ALLOCATE capability is supported
// client can ask to test the allocation by passing NULL out_buffers
bool test_allocate = !out_buffers;
// Validate descriptors
std::lock_guard<std::mutex> descriptor_lock(descriptor_lock_);
std::vector<std::shared_ptr<BufferDescriptor>> descriptors;
for (uint32_t i = 0; i < num_descriptors; i++) {
const auto map_descriptor = descriptors_map_.find(descriptor_ids[i]);
if (map_descriptor == descriptors_map_.end()) {
return GRALLOC1_ERROR_BAD_DESCRIPTOR;
} else {
descriptors.push_back(map_descriptor->second);
}
}
// Resolve implementation defined formats
for (auto &descriptor : descriptors) {
descriptor->SetColorFormat(allocator_->GetImplDefinedFormat(descriptor->GetProducerUsage(),
descriptor->GetConsumerUsage(),
descriptor->GetFormat()));
}
// Check if input descriptors can be supported AND
// Find out if a single buffer can be shared for all the given input descriptors
uint32_t i = 0;
ssize_t max_buf_index = -1;
shared = allocator_->CheckForBufferSharing(num_descriptors, descriptors, &max_buf_index);
if (test_allocate) {
status = shared ? GRALLOC1_ERROR_NOT_SHARED : status;
return status;
}
std::lock_guard<std::mutex> buffer_lock(buffer_lock_);
if (shared && (max_buf_index >= 0)) {
// Allocate one and duplicate/copy the handles for each descriptor
if (AllocateBuffer(*descriptors[UINT(max_buf_index)], &out_buffers[max_buf_index])) {
return GRALLOC1_ERROR_NO_RESOURCES;
}
for (i = 0; i < num_descriptors; i++) {
// Create new handle for a given descriptor.
// Current assumption is even MetaData memory would be same
// Need to revisit if there is a need for own metadata memory
if (i != UINT(max_buf_index)) {
CreateSharedHandle(out_buffers[max_buf_index], *descriptors[i], &out_buffers[i]);
}
}
} else {
// Buffer sharing is not feasible.
// Allocate separate buffer for each descriptor
for (i = 0; i < num_descriptors; i++) {
if (AllocateBuffer(*descriptors[i], &out_buffers[i])) {
return GRALLOC1_ERROR_NO_RESOURCES;
}
}
}
// Allocation is successful. If backstore is not shared inform the client.
if (!shared) {
return GRALLOC1_ERROR_NOT_SHARED;
}
return status;
}
// Sort selected entries to the top of the list
void buffer_sort_selected(DirBuffer *buffer)
{
struct MinList temp;
DirEntry *entry,*first_file=0,*first_pos=0;
// Lock buffer
buffer_lock(buffer,TRUE);
// Initialise temporary list
NewList((struct List *)&temp);
// Go through entries
for (entry=(DirEntry *)buffer->entry_list.mlh_Head;
entry->de_Node.dn_Succ;)
{
DirEntry *next=(DirEntry *)entry->de_Node.dn_Succ;
// File?
if (ENTRYTYPE(entry->de_Node.dn_Type)==ENTRY_FILE)
{
// Remove and add to temporary if selected
if (entry->de_Flags&ENTF_SELECTED)
{
// Remember first position
if (!first_pos) first_pos=(DirEntry *)entry->de_Node.dn_Pred;
// Remove and re-add
Remove((struct Node *)entry);
AddTail((struct List *)&temp,(struct Node *)entry);
}
// Remember if the first file
else
if (!first_file) first_file=entry;
}
// Get next
entry=next;
}
// Something in the temporary list?
if (!(IsListEmpty((struct List *)&temp)))
{
// If we have a first file, get its predecessor
if (first_file) first_pos=(DirEntry *)first_file->de_Node.dn_Pred;
// Go through temporary list, add after first position
for (entry=(DirEntry *)temp.mlh_Head;
entry->de_Node.dn_Succ;)
{
DirEntry *next=(DirEntry *)entry->de_Node.dn_Succ;
// Remove and add after last one
Remove((struct Node *)entry);
Insert((struct List *)&buffer->entry_list,(struct Node *)entry,(struct Node *)first_pos);
// Bump next position
first_pos=(DirEntry *)entry;
// Get next
entry=next;
}
}
// Unlock buffer
buffer_unlock(buffer);
}
