chat_window::chat_window(std::string name)
{
name_ = name;
if(name.c_str()[0] == '#')
{
msg_window_ = new msg_window(this);
msg_box_ = new msg_box(this);
contact_list_ = new user_list_window(this);
msg_window_->set_size(0,0,400,400);
msg_box_->set_size(0,420,400,50);
contact_list_->set_size(450,0,150,400);
//Denna som ska användas sen!!!!!
//connect(contact_list_,SIGNAL(slap_signal(std::string)),msg_box_,SLOT(send_msg(std::string)));
connect(contact_list_,SIGNAL(slap_signal(std::string)),this,SLOT(new_msg(std::string)));
}
else
{
msg_window_ = new msg_window(this);
msg_box_ = new msg_box(this);
msg_window_->set_size(0,0,400,400);
msg_box_->set_size(0,420,400,50);
}
msg_window_->new_message("Babla>> ","Välkommen till IRC-klienten Babla 0.1 Beta\n");
msg_window_->new_message("Babla>> ","För hjälp skriv /help\n");
}
int main(int argc, char ** argv)
{
struct sched_param sp; /* Priorytet procesu */
/* Zainicjuj obiekty. Ponieważ jest to pierwsze użycie - wykonają sie wszystkie funcje malloc i pliki zostaną otwarte */
struct controller_t * c = new_controller();
struct msg_t * msg = new_msg();
struct io_t * io = new_io();
/* Ustawianie parametrów planisty */
sp.sched_priority = MY_PRIORITY;
if (sched_setscheduler(0, SCHED_FIFO, &sp) == -1)
{
log("sched_setscheduler() failed");
}
/* Blokowanie pamięci procesu */
if (mlockall( MCL_CURRENT|MCL_FUTURE ) == -1)
{
log("mlockall() failed");
}
while (1)
{
/* Wykonaj krok automatu */
do_fsm_step();
}
return 0;
}
void PolygonArrayColorLikelihood::likelihood(
const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg,
const jsk_recognition_msgs::HistogramWithRangeArray::ConstPtr& histogram_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!reference_) {
return;
}
if (polygon_msg->polygons.size() != histogram_msg->histograms.size()) {
JSK_NODELET_ERROR("length of polygon and histogram are not same");
return;
}
cv::MatND reference_histogram
= jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND(
reference_->bins);
cv::normalize(reference_histogram, reference_histogram, 1, reference_histogram.rows, cv::NORM_L2,
-1, cv::Mat());
jsk_recognition_msgs::PolygonArray new_msg(*polygon_msg);
for (size_t i = 0; i < new_msg.polygons.size(); i++) {
cv::MatND hist
= jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND(
histogram_msg->histograms[i].bins);
cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2,
-1, cv::Mat());
double d = compareHist(reference_histogram, hist);
if (polygon_msg->likelihood.size() == 0) {
new_msg.likelihood.push_back(d);
}
else {
new_msg.likelihood[i] *= d;
}
}
pub_.publish(new_msg);
}
void PolygonArrayAreaLikelihood::likelihood(
const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
jsk_recognition_msgs::PolygonArray new_msg(*msg);
double min_area = DBL_MAX;
double max_area = - DBL_MAX;
std::vector<double> areas;
for (size_t i = 0; i < msg->polygons.size(); i++) {
jsk_recognition_utils::Polygon::Ptr polygon
= jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
double area = polygon->area();
min_area = std::min(area, min_area);
max_area = std::max(area, max_area);
areas.push_back(area);
}
// Normalization
for (size_t i = 0; i < areas.size(); i++) {
double diff = areas[i] - area_;
double likelihood = 1 / (1 + (diff * diff));
if (msg->likelihood.size() == 0) {
new_msg.likelihood.push_back(likelihood);
}
else {
new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
}
}
pub_.publish(new_msg);
}
/**
* send message to given destination
*/
asmlinkage long sys_SendMsg(pid_t dest, void *a_msg, int len, bool block){
pid_t my_pid = current->pid;
void* msg = new_msg();
message* this_mail;
mailbox* dest_mailbox;
signal* dest_signal;
struct task_struct* dest_ts;
int existence;
if ((len > MAX_MSG_SIZE) || (len < 0))
return MSG_LENGTH_ERROR;
if (copy_from_user(msg, a_msg, len))
return MSG_ARG_ERROR;
//check if destination is valid
if (dest <= 0) return MAILBOX_INVALID;
//find task struct for destination pid
dest_ts = pid_task(find_vpid(dest), PIDTYPE_PID);
// find_task_by_vpid(dest);
if (dest_ts == NULL) return MAILBOX_INVALID;
//state not 0 or kernel task, invalid dest
existence = dest_ts->state;
if ((existence != 0) || (dest_ts->mm == NULL)) return MAILBOX_INVALID;
//get destination mailbox
dest_signal = get_signal(dest);
if (dest_signal == NULL) {
dest_signal = create_signal(dest, TRUE);
}
dest_mailbox = get_mailbox(dest);
if (dest_mailbox == NULL) {
dest_mailbox = create_mailbox(dest);
if (dest_mailbox == NULL) return MAILBOX_ERROR;
}
wake_up(&(dest_signal->wait_null));
if ((block == TRUE) && (dest_mailbox->full == TRUE)){
//wait until not full and send message
}
else if (block == FALSE && (dest_mailbox->full == TRUE))
return MAILBOX_FULL; if (dest_mailbox->stop)
return MAILBOX_STOPPED;
this_mail = create_message(my_pid, len, msg);
spin_lock(&(dest_mailbox->lock));
add_message(&dest_mailbox, &this_mail);
spin_unlock(&(dest_mailbox->lock));
//successfully sent
return 0;
}
void PolygonArrayAngleLikelihood::likelihood(
const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
jsk_recognition_msgs::PolygonArray new_msg(*msg);
try
{
// Resolve tf
// ConstPtrmpute position of target_frame_id
// respected from msg->header.frame_id
tf::StampedTransform transform;
tf_listener_->lookupTransform(
target_frame_id_, msg->header.frame_id, msg->header.stamp, transform);
Eigen::Affine3f pose;
tf::transformTFToEigen(transform, pose);
// Use x
Eigen::Vector3f reference_axis = pose.rotation() * axis_;
double min_distance = DBL_MAX;
double max_distance = - DBL_MAX;
std::vector<double> distances;
for (size_t i = 0; i < msg->polygons.size(); i++) {
jsk_recognition_utils::Polygon::Ptr polygon
= jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon);
Eigen::Vector3f n = polygon->getNormal();
double distance = std::abs(reference_axis.dot(n));
min_distance = std::min(distance, min_distance);
max_distance = std::max(distance, max_distance);
distances.push_back(distance);
}
// Normalization
for (size_t i = 0; i < distances.size(); i++) {
// double normalized_distance
// = (distances[i] - min_distance) / (max_distance - min_distance);
double likelihood = 1 / (1 + (distances[i] - 1) * (distances[i] - 1));
if (msg->likelihood.size() == 0) {
new_msg.likelihood.push_back(likelihood);
}
else {
new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood;
}
}
pub_.publish(new_msg);
}
catch (...)
{
JSK_NODELET_ERROR("Unknown transform error");
}
}
void base_controller::twist_cb( const geometry_msgs::TwistPtr &msg )
{
trajectory_msgs::JointTrajectoryPtr new_msg( new trajectory_msgs::JointTrajectory( joint_traj_template ) );
new_msg->header.stamp = ros::Time::now( );
new_msg->points[0].velocities[0] = ( 2 * msg->linear.x - msg->angular.z * wheel_base ) / wheel_diam;
new_msg->points[0].velocities[1] = ( 2 * msg->linear.x + msg->angular.z * wheel_base ) / wheel_diam2;
joint_traj_pub.publish( new_msg );
}
void operator()(msg_type & msg) const {
typedef typename context_type::ptr ptr;
typedef typename context_type::thread_ptr thread_ptr;
//ptr new_msg(new msg_type(msg));
ptr new_msg(new msg_type);
std::swap(msg, *new_msg);
scoped_lock lk( non_const_context_base::context().cs);
if ( !non_const_context_base::context().writer)
non_const_context_base::context().writer = thread_ptr( new boost::thread( boost::bind(&self_type::do_write,this) ));
non_const_context_base::context().msgs.push_back(new_msg);
}
int main(int argc, char **argv)
{
int clientfd, port;
char *host, buf[MAX_LINE];
if (argc == 2 || argc == 3) {
host = argv[1];
port = (argc == 2) ? DEFAULT_PORT : atoi(argv[2]);
}
else {
fprintf(stderr, "usage: %s <host>\n", argv[0]);
exit(0);
}
clientfd = Open_clientfd(host, port);
printf("Bank client v.%d connected to %s on port %d\n", VERSION, host, port);
msg_t *request = new_msg();
msg_t *response = new_msg();
printf("> "); fflush(stdout);
while (Fgets(buf, MAX_LINE, stdin) != NULL) {
if (parse_buf(buf, request)) {
Rio_writen(clientfd, (void *) request, sizeof(msg_t)); // Send bits to server
Rio_readn(clientfd, (void *) response, sizeof(msg_t)); // Get bits back from server
print_response(response);
}
clear_msg(request);
clear_msg(response);
printf("> "); fflush(stdout);
}
Close(clientfd);
free(request);
free(response);
exit(0);
}
static int
update_msg(void)
{
static time_t yesterday = 0;
time_t today = time(NULL) / PERIOD_RENEWAL;
if (yesterday == 0)
yesterday = today;
if (today != yesterday || actual_msg[0] == '\0') {
yesterday = today;
new_msg();
return 1;
}
return 0;
}
static void Send(kn_thread_mailbox_t io,ident conn,packet_t p){
msg_t _msg = new_msg(MSG_PACKET,conn,p,NULL);
if(0 != kn_send_mail(io,_msg,destroy_msg)){
destroy_msg(_msg);
}
}
void on_accept(handle_t s,void *listener,int _2,int _3){
connection_t conn = new_connection(s,4096,NULL);
msg_t _msg = new_msg(MSG_CONNECTION,make_ident((refobj*)conn),NULL,NULL);
ioworker_sendmsg(_worker,_msg);
}
static int client_process(int max, fd_set* set, int remotefd, int localfd)
{
msg_t* msg;
if (FD_ISSET(localfd, set))
{
unsigned char buffer[2048];
ssize_t readen;
readen = read(localfd, buffer, sizeof(buffer));
if (readen > 0)
{
msg = new_msg(buffer, readen);
if (msg)
{
write_n(remotefd, msg, sizeof(msg_t) + msg_data_length(msg));
pool_room_free(&this.pool, MSG_ROOM_IDX);
printf("send msg length: %lu\n", msg_data_length(msg));
}
}
}
if (FD_ISSET(remotefd, set))
{
ssize_t rc = read_pre(remotefd, this.client.read, this.client.want);
if (rc == 0)
{
fprintf(stderr, "connection closed\n");
return RETURN_CONNECTION_CLOSED;
}
else if (rc < 0)
{
if (errno == EAGAIN || errno == EWOULDBLOCK) return RETURN_OK;
fprintf(stderr, "read error\n");
perror("read");
return RETURN_READ_ERROR;
}
else
{
this.client.read += rc;
this.client.want -= rc;
if (this.client.want == 0)
{
if (IS_CLIENT_STATUS_WAITING_HEADER(this.client.status))
{
size_t len = msg_data_length((msg_t*)this.client.buffer);
if (len)
{
this.client.status = (this.client.status & ~CLIENT_STATUS_WAITING_HEADER) | CLIENT_STATUS_WAITING_BODY;
this.client.want = len;
this.client.buffer = pool_room_realloc(&this.pool, RECV_ROOM_IDX, sizeof(msg_t) + this.client.want);
if (this.client.buffer == NULL)
{
fprintf(stderr, "Not enough memory\n");
exit(1);
}
this.client.read = ((msg_t*)this.client.buffer)->data;
}
else process_msg((msg_t*)this.client.buffer, localfd);
}
else process_msg((msg_t*)this.client.buffer, localfd);
}
}
}
return RETURN_OK;
}
/********************************************************************
* FUNCTION mgr_not_dispatch
*
* Dispatch an incoming <rpc-reply> response
* handle the <notification> element
* called by mgr_top.c:
* This function is registered with top_register_node
* for the module 'notification', top-node 'notification'
*
* INPUTS:
* scb == session control block
* top == top element descriptor
*********************************************************************/
void
mgr_not_dispatch (ses_cb_t *scb,
xml_node_t *top)
{
#ifdef DEBUG
if (!scb || !top) {
SET_ERROR(ERR_INTERNAL_PTR);
return;
}
#endif
/* check if the notification template is already cached */
if (notification_obj == NULL) {
log_error("\nNo notification module found\n");
mgr_xml_skip_subtree(scb->reader, top);
return;
}
/* the current node is 'notification' in the notifications namespace
* First get a new notification msg struct
*/
mgr_not_msg_t *msg = new_msg();
if (!msg) {
log_error("\nError: mgr_not: skipping incoming message");
mgr_xml_skip_subtree(scb->reader, top);
return;
}
/* parse the notification as a val_value_t tree,
* stored in msg->notification
*/
msg->res = mgr_val_parse_notification(scb, notification_obj,
top, msg->notification);
if (msg->res != NO_ERR && LOGINFO) {
log_info("\nmgr_not: got invalid notification on session %d (%s)",
scb->sid, get_error_string(msg->res));
}
/* check that there is nothing after the <rpc-reply> element */
if (msg->res==NO_ERR && !xml_docdone(scb->reader) && LOGINFO) {
log_info("\nmgr_not: got extra nodes in notification on session %d",
scb->sid);
}
boolean consumed = FALSE;
if (msg->res == NO_ERR && msg->notification) {
val_value_t *child = val_get_first_child(msg->notification);
if (child) {
if (!xml_strcmp(child->name,
(const xmlChar *)"eventTime")) {
msg->eventTime = child;
} else {
log_error("\nError: expected 'eventTime' in "
"notification, got '%s'", child->name);
}
child = val_get_next_child(child);
if (child) {
/* eventType is expected to be next!! */
msg->eventType = child;
}
} else {
log_error("\nError: expected 'eventTime' in "
"notification, got nothing");
}
/* invoke the notification handler */
if (callbackfn) {
(*callbackfn)(scb, msg, &consumed);
}
}
if (!consumed) {
mgr_not_free_msg(msg);
}
} /* mgr_not_dispatch */
void Reassemble::recv (Message_ptr m)
{
Map::ENTRY* e = 0;
Address from (
static_cast<From const*> (m->find (From::id))->address ());
if (Data const* data = static_cast<Data const*> (m->find (Data::id)))
{
if (Part const* part = static_cast<Part const*> (m->find (Part::id)))
{
if (map_.find (from, e) == -1)
{
// First part of the message.
//
if (part->num () != 1)
{
// We assume that we received NoData for one of the preceding
// fragments. Ignore this one.
return;
}
Data_ptr new_data (new Data (data->buf (),
static_cast<size_t> (data->size ()),
static_cast<size_t> (part->total_size ())));
//std::cerr << "part->total_size (): " << part->total_size () << endl;
map_.bind (from, new_data);
}
else
{
// Next part of the message.
//
if (part->num () == 1)
ACE_OS::abort ();
Data const* data = static_cast<Data const*> (m->find (Data::id));
Data_ptr& new_data = e->int_id_;
ACE_OS::memcpy (new_data->buf () + new_data->size (),
data->buf (),
data->size ());
//std::cerr << "data->size (): " << data->size () << endl
// << "new_data->size (): " << new_data->size () << endl
// << "new_data->capa (): " << new_data->capacity () << endl;
new_data->size (new_data->size () + data->size ());
if (part->num () == part->of ())
{
// Reassembly is complete.
//
if (part->total_size () != new_data->size ())
ACE_OS::abort ();
Message_ptr new_msg (new Message ());
Address to (
static_cast<To const*> (m->find (To::id))->address ());
new_msg->add (Profile_ptr (new To (to)));
new_msg->add (Profile_ptr (new From (from)));
/*
* Heads up... we need to add the new_data to new_msg then
* unbind the entry that maps to new_data, which will decrement
* its reference count. If the bound/refcounted pointer acted
* polymorphically like a regular pointer does, we'd be able to
* just pass new_data to add(Profile_Ptr) and it would work.
* However, Profile_Ptr and Data_Ptr are not compatible, but
* we can use the secret knowledge that both are instances of the
* same template and that the pointers they contain really are
* hierarchically compatible, and do this funky cast to get
* the result we want.
*/
//new_msg->add (*(reinterpret_cast<Profile_ptr*> (&new_data)));
new_msg->add (Profile_ptr (new_data));
map_.unbind (from);
in_->recv (new_msg);
}
}
}
else
{
// Non-fragmented message. Make sure we are in the consistent state
// and forward it up.
//
if (map_.find (from, e) != -1)
ACE_OS::abort ();
in_->recv (m);
}
}
else if (m->find (NoData::id) != 0)
{
if (map_.find (from, e) != -1)
{
// We already received some fragments. Clean everyhting up.
//
map_.unbind (from);
}
in_->recv (m);
}
}