os_lock_t os_create_lock(void)
{
os_lock_pvt_t *p_lock = NULL;
if(NULL == (p_lock = OS_ALLOC(sizeof(os_lock_pvt_t)))) {
OS_ERROR("OS_ALLOC failed\n");
goto error;
}
if(NULL == (p_lock->p_sema = OS_ALLOC(sizeof(os_sema_t)))) {
OS_ERROR("OS_ALLOC failed\n");
goto error;
}
os_sema_init(p_lock->p_sema, 1);
return ((os_lock_t)p_lock);
error:
if(p_lock != NULL) {
OS_FREE(p_lock);
}
return (NULL);
}
Status setup_signals_alt_stack() {
#if TD_PORT_POSIX && !TD_DARWIN_TV_OS && !TD_DARWIN_WATCH_OS
auto page_size = getpagesize();
auto stack_size = (MINSIGSTKSZ + 16 * page_size - 1) / page_size * page_size;
void *stack = mmap(nullptr, stack_size + 2 * page_size, PROT_READ | PROT_WRITE, MAP_PRIVATE | MAP_ANON, -1, 0);
if (stack == MAP_FAILED) {
return OS_ERROR("Mmap failed");
}
TRY_STATUS(protect_memory(stack, page_size));
TRY_STATUS(protect_memory(static_cast<char *>(stack) + stack_size + page_size, page_size));
stack_t signal_stack;
signal_stack.ss_sp = static_cast<char *>(stack) + page_size;
signal_stack.ss_size = stack_size;
signal_stack.ss_flags = 0;
if (sigaltstack(&signal_stack, nullptr) != 0) {
return OS_ERROR("sigaltstack failed");
}
#endif
return Status::OK();
}
/* OpenCV mouse callback function.
*
* LeftButton: get the under-mouse pixel color
* LeftButton + Ctrl:
*/
void mouse_callback(int event, int x, int y, int flags, void* param) {
cv::Mat *img = (cv::Mat *)param;
if(event == CV_EVENT_LBUTTONDOWN) {
if(flags & CV_EVENT_FLAG_CTRLKEY) {
obj_rec::addPercept srv;
srv.request.id = percept_id;
if (addPercept_client->call(srv)) {
ROS_INFO("percept %d - %d", srv.request.id, (unsigned char)srv.response.ok);
} else {
OS_ERROR("Failed to add percept");
return ;
}
} else {
// read from image
cv::Vec<unsigned char,4> col = img->at<cv::Vec<unsigned char,4> >(y, x);
obj_rec::addColor srv;
srv.request.rr = col[2];
srv.request.gg = col[1];
srv.request.bb = col[0];
srv.request.id = color_id;
if (addColor_client->call(srv)) {
ROS_INFO("pixel (%d,%d): %d,%d,%d - %d ? %d", x, y, srv.request.rr, srv.request.gg, srv.request.bb, color_id, (unsigned char)srv.response.ok);
} else {
ROS_ERROR("Failed to add pixel");
return ;
}
}
} else if(event == CV_EVENT_RBUTTONDOWN) {
if(flags & CV_EVENT_FLAG_CTRLKEY) {
percept_id = (percept_id+1)%8;
} else {
color_id = (color_id+1)%8;
}
}
}
static Status set_signal_handler_impl(vector<int> signals, F func, bool is_extended = false) {
struct sigaction act;
std::memset(&act, '\0', sizeof(act));
if (is_extended) { // TODO if constexpr, remove useless reinterpret_cast
act.sa_handler = reinterpret_cast<decltype(act.sa_handler)>(func);
} else {
act.sa_sigaction = reinterpret_cast<decltype(act.sa_sigaction)>(func);
}
sigemptyset(&act.sa_mask);
for (auto signal : signals) {
sigaddset(&act.sa_mask, signal);
}
act.sa_flags = SA_RESTART | SA_ONSTACK;
if (is_extended) {
act.sa_flags |= SA_SIGINFO;
}
for (auto signal : signals) {
if (sigaction(signal, &act, nullptr) != 0) {
return OS_ERROR("sigaction failed");
}
}
return Status::OK();
}
static Status protect_memory(void *addr, size_t len) {
if (mprotect(addr, len, PROT_NONE) != 0) {
return OS_ERROR("mprotect failed");
}
return Status::OK();
}
