int smem_create(char *filename, int *driverhandle)
{ DAL_SHM_SEGHEAD *sp;
int h, sz, nitems;
if (NULL == filename) return(SHARED_NULPTR); /* currently ignored */
if (NULL == driverhandle) return(SHARED_NULPTR);
nitems = sscanf(filename, "h%d", &h);
if (1 != nitems) return(SHARED_BADARG);
if (SHARED_INVALID == (h = shared_malloc(sz = 2880 + sizeof(DAL_SHM_SEGHEAD),
SHARED_RESIZE | SHARED_PERSIST, h)))
return(SHARED_NOMEM);
if (NULL == (sp = (DAL_SHM_SEGHEAD *)shared_lock(h, SHARED_RDWRITE)))
{ shared_free(h);
return(SHARED_BADARG);
}
sp->ID = DAL_SHM_SEGHEAD_ID;
sp->h = h;
sp->size = sz;
sp->nodeidx = -1;
*driverhandle = h;
return(0);
}
int smem_open(char *filename, int rwmode, int *driverhandle)
{ int h, nitems, r;
DAL_SHM_SEGHEAD *sp;
if (NULL == filename) return(SHARED_NULPTR);
if (NULL == driverhandle) return(SHARED_NULPTR);
nitems = sscanf(filename, "h%d", &h);
if (1 != nitems) return(SHARED_BADARG);
if (SHARED_OK != (r = shared_attach(h))) return(r);
if (NULL == (sp = (DAL_SHM_SEGHEAD *)shared_lock(h,
((READWRITE == rwmode) ? SHARED_RDWRITE : SHARED_RDONLY))))
{ shared_free(h);
return(SHARED_BADARG);
}
if ((h != sp->h) || (DAL_SHM_SEGHEAD_ID != sp->ID))
{ shared_unlock(h);
shared_free(h);
return(SHARED_BADARG);
}
*driverhandle = h;
return(0);
}
int shared_uncond_delete(int id)
{ int i, r;
if (NULL == shared_gt) return(SHARED_NOTINIT); /* not initialized */
if (NULL == shared_lt) return(SHARED_NOTINIT); /* not initialized */
if (shared_debug) printf("shared_uncond_delete:");
r = SHARED_OK;
for (i=0; i<shared_maxseg; i++)
{ if (-1 != id) if (i != id) continue;
if (shared_attach(i))
{ if (-1 != id) printf("no such handle\n");
continue;
}
printf("handle %d:", i);
if (NULL == shared_lock(i, SHARED_RDWRITE | SHARED_NOWAIT))
{ printf(" cannot lock in RW mode, not deleted\n");
continue;
}
if (shared_set_attr(i, SHARED_RESIZE) >= SHARED_ERRBASE)
{ printf(" cannot clear PERSIST attribute");
}
if (shared_free(i))
{ printf(" delete failed\n");
}
else
{ printf(" deleted\n");
}
}
if (shared_debug) printf(" done\n");
return(r); /* table full */
}
bool blockingGet(const K& key, V& value) {
boost::shared_lock<boost::shared_mutex> shared_lock(_mutex);
typename boost::unordered_map<K, V>::iterator it = this->find(key);
if (it == this->end()) {
return false;
}
value = (*this)[key];
return true;
}
bool blockingGet(const K& key, V& value) {
size_t index = _hasher(key) % _size;
boost::shared_lock<boost::shared_mutex> shared_lock(_mutexs[index]);
typename boost::unordered_map<K, V>::iterator it = _maps[index].find(key);
if (it == _maps[index].end()) {
return false;
}
value = _maps[index][key];
return true;
}
void LocalFiberRef::send(const PendingEvent& pendingEvent) {
boost::shared_lock<boost::upgrade_mutex> shared_lock(block->mutex);
block->mailbox->enqueue(pendingEvent);
if (block->status == Suspended) {
boost::upgrade_lock<boost::upgrade_mutex> upgrade_lock(std::move(shared_lock), boost::try_to_lock);
if (upgrade_lock.owns_lock()) {
boost::unique_lock<boost::upgrade_mutex> unique_lock(std::move(upgrade_lock));
system->schedule(block, std::move(unique_lock));
}
}
}
Value get(const CalculateFunction& calculate)
{
boost::upgrade_lock<boost::upgrade_mutex> shared_lock(mContainerMutex);
if (mHas)
return mValue;
// This is controversial but we prefer to block any other access while
// calculating. This will prevent double calculation.
boost::upgrade_to_unique_lock<boost::upgrade_mutex> unique_lock(shared_lock);
mHas = true;
return mValue = calculate();
}
//=============================================================================
//------------------------------do_monitor_enter-------------------------------
void Parse::do_monitor_enter() {
kill_dead_locals();
// Null check; get casted pointer.
Node *obj = do_null_check(peek(), T_OBJECT);
// Check for locking null object
if (stopped()) return;
// the monitor object is not part of debug info expression stack
pop();
// Insert a FastLockNode which takes as arguments the current thread pointer,
// the obj pointer & the address of the stack slot pair used for the lock.
shared_lock(obj);
}
Value get(const HandleSPtr& handle, Args... args, const CalculateFunction& calculate)
{
const auto& key = mContainer.key(handle, args...);
boost::upgrade_lock<boost::shared_mutex> shared_lock(mContainerMutex);
const auto& holder = mContainer.holder(key);
if (mContainer.has(holder))
return mContainer.get(holder);
// This is controversial but we prefer to block any other access while
// calculating. This will prevent double calculation.
boost::upgrade_to_unique_lock<boost::shared_mutex> unique_lock(shared_lock);
const Value& value = calculate(args...);
mContainer.put(key, value);
return value;
}
int smem_remove(char *filename)
{ int nitems, h, r;
if (NULL == filename) return(SHARED_NULPTR);
nitems = sscanf(filename, "h%d", &h);
if (1 != nitems) return(SHARED_BADARG);
if (0 == shared_check_locked_index(h)) /* are we locked ? */
{ if (-1 != shared_lt[h].lkcnt) /* are we locked RO ? */
{ if (SHARED_OK != (r = shared_unlock(h))) return(r); /* yes, so relock in RW */
if (NULL == shared_lock(h, SHARED_RDWRITE)) return(SHARED_BADARG);
}
}
else /* not locked */
{ if (SHARED_OK != (r = smem_open(filename, READWRITE, &h)))
return(r); /* so open in RW mode */
}
shared_set_attr(h, SHARED_RESIZE); /* delete PERSIST attribute */
return(smem_close(h)); /* detach segment (this will delete it) */
}
/// Gibt einen #shared_lock, initialisiert mit #mtx, zurück
shared_lock read_lock() {
return shared_lock( mtx );
}
void CommandSubscriber::commandCallback(const kinfu_msgs::KinfuCommand & cmd)
{
boost::mutex::scoped_lock shared_lock(m_shared_mutex);
if (cmd.command_type == cmd.COMMAND_TYPE_NOOP)
{
// does nothing
ack(cmd.command_id,true);
}
else if (cmd.command_type == cmd.COMMAND_TYPE_RESUME)
{
m_is_running = true;
ack(cmd.command_id,true);
}
else if (cmd.command_type == cmd.COMMAND_TYPE_SUSPEND)
{
m_is_running = false;
ack(cmd.command_id,true);
}
else if (cmd.command_type == cmd.COMMAND_TYPE_RESET)
{
if (m_request_reset)
ack(m_request_reset_command_id,false);
m_request_reset = true;
m_request_reset_command_id = cmd.command_id;
}
else if (cmd.command_type == cmd.COMMAND_TYPE_SET_ENABLED_MIN_MOVEMENT)
{
if (cmd.boolean_data.size() < 1)
{
ROS_ERROR("kinfu: command: set enabled min movement: one boolean required in boolean_data.");
ack(cmd.command_id,false);
}
else
{
m_enable_minimum_movement = cmd.boolean_data[0];
ROS_INFO("kinfu: command: set enable min movement: %u.",uint(m_enable_minimum_movement));
ack(cmd.command_id,true);
}
}
else if (cmd.command_type == cmd.COMMAND_TYPE_SET_FORCED_TF_FRAMES)
{
if (cmd.boolean_data.size() < 1)
{
ROS_ERROR("kinfu: command: set forced tf frame: one boolean required in boolean_data.");
ack(cmd.command_id,false);
}
else
{
m_forced_tf_frames = cmd.boolean_data[0];
ROS_INFO("kinfu: command: set forced tf frame: %u.",uint(m_forced_tf_frames));
if (m_forced_tf_frames)
m_hint_expiration = ros::Time(0); // clear the hint: now forced from tf
ack(cmd.command_id,true);
}
}
else if (cmd.command_type == cmd.COMMAND_TYPE_CLEAR_SPHERE)
{
ROS_INFO("kinfu: command: clear sphere.");
if (cmd.float_data.size() < 4)
ROS_ERROR("kinfu: command: a sphere requires 4 float_data params.");
else
{
Eigen::Vector3f center,kinfu_center;
float radius;
for (uint i = 0; i < 3; i++)
center[i] = cmd.float_data[i];
radius = cmd.float_data[3];
kinfu_center = m_initial_transformation * center;
m_clear_sphere = Sphere::Ptr(new Sphere(kinfu_center,radius,cmd.command_id));
}
}
else if (cmd.command_type == cmd.COMMAND_TYPE_CLEAR_BOUNDING_BOX)
{
ROS_INFO("kinfu: command: clear bbox.");
if (cmd.float_data.size() < 6)
ROS_ERROR("kinfu: command: a sphere requires 6 float_data params.");
else
{
Eigen::Vector3f min,max;
Eigen::Vector3f kinfu_min,kinfu_max;
for (uint i = 0; i < 3; i++)
min[i] = cmd.float_data[i];
for (uint i = 0; i < 3; i++)
max[i] = cmd.float_data[i + 3];
kinfu_min = m_initial_transformation * min;
kinfu_max = m_initial_transformation * max;
m_clear_bbox = BBox::Ptr(new BBox(kinfu_min,kinfu_max,cmd.command_id));
}
}
else if (cmd.command_type == cmd.COMMAND_TYPE_TRIGGER)
{
if (m_is_running || m_is_triggered)
ROS_WARN("kinfu: trigger request ignored: kinfu is already running.");
else
{
m_is_triggered = true;
m_is_triggered_command_id = cmd.command_id;
}
}
else
{
ROS_ERROR("kinfu: command: unknown command type: %u",uint(cmd.command_type));
ack(cmd.command_id,false);
}
// if the hint comes from tf frames, do not force hint otherwise
if (!m_forced_tf_frames)
{
if (cmd.hint_expiration_time >= ros::Time::now())
{
m_hint_expiration = cmd.hint_expiration_time;
for (uint r = 0; r < 3; r++)
for (uint c = 0; c < 3; c++)
m_hint.linear()(r,c) = cmd.pose_hint_rotation[r * 3 + c];
for (uint i = 0; i < 3; i++)
m_hint.translation()[i] = cmd.pose_hint_translation[i];
m_hint = m_initial_transformation * m_hint;
m_hint_forced = cmd.hint_forced;
}
}
}
