Project: engagement_recognition License: BSD Dependencies: engagement_msgs engagement_srvs roscpp Used by: None All Packages

engagement_recognition/src/recognition/rnode/rnode.cpp Go to the documentation of this file. /* * Software License Agreement (BSD License) * * Copyright (c) 2010, Worcester Polytechnic Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Worcester Polytechnic Institute. nor the names * of its contributors may be used to endorse or promote products * derived from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ // @author Brett Ponsler (bponsler (at) wpi (dot) edu) #include "rnode.h" // Include additional functions that may be useful #include "../actors/actor_manager.h" #include "../lib/engagement_macros.h" #include "../lib/engagement_params.h" #include "../objects/engagement_objects.h" // Include the engagement handler which is used for each Actor #include "../handlers/engagement_handler.h" // Include the RecognizerFactory and the EventFactory #include "../recognizers/recognizer_factory.h" #include "../events/event_factory.h" // Include all the subscription callback methods #include "callbacks/performing.h" #include "callbacks/recognition.h" #include "callbacks/vision.h" // Include the file logger #include "../logger/file_logger.h" RNode::~RNode() { // Deconstruct the robot ActorManager::destroy(); Robot::destroy(); // Delete the master thread if (this->master_thread_) free(master_thread_); } RNode::RNode() : EventSink(), master_thread_(NULL), old_threads_() { // Create the logger for this class this->setLogger(EngagementParams::getRecogTag()); FileLogger::setDirectory(EngagementParams::getLogDirectory()); // Set the default RNode values here this->shared_gaze_start_ = UNDEFINED_TIME; this->shared_gaze_ = Duration(); // Subscribe and advertise all the incoming and outgoing messages this->subscribe(); this->advertise(); } RNode::RNode(EventSink *parent) : EventSink(parent) { // Create the logger for this class this->setLogger(EngagementParams::getRecogTag()); FileLogger::setDirectory(EngagementParams::getLogDirectory()); // Set the default RNode values here this->shared_gaze_start_ = UNDEFINED_TIME; this->shared_gaze_ = Duration(); // Subscribe and advertise all the incoming and outgoing messages this->subscribe(); this->advertise(); } void RNode::subscribe() { // Create the node handles for vision and collaboration ros::NodeHandle *vision_handle = EngagementParams::getVisionHandle(); ros::NodeHandle *performing_handle = EngagementParams::getPerformingHandle(); // Subscribe using the control handle performing_look_sub_ = RNODE_SUBSCRIBE(performing_handle, PERFORMING_LOOK_TOPIC, &RNode::performingLookCallback); performing_point_sub_ = RNODE_SUBSCRIBE(performing_handle, PERFORMING_POINT_TOPIC, &RNode::performingPointCallback); performing_shake_sub_ = RNODE_SUBSCRIBE(performing_handle, PERFORMING_SHAKE_TOPIC, &RNode::performingShakeCallback); performing_nod_sub_ = RNODE_SUBSCRIBE(performing_handle, PERFORMING_NOD_TOPIC, &RNode::performingNodCallback); performing_ext_action_sub_ = RNODE_SUBSCRIBE(performing_handle, PERFORMING_EXTENDED_ACTION_TOPIC, &RNode::performingExtendedActionCallback); performing_utterance_sub_ = RNODE_SUBSCRIBE(performing_handle, PERFORMING_UTTERANCE_TOPIC, &RNode::performingUtteranceCallback); // Subscribe using the vision handle vision_face_sub_ = RNODE_SUBSCRIBE(vision_handle, VISION_FACE_TOPIC, &RNode::visionFaceCallback); } void RNode::advertise() { // Grab the NodeHandles for external messages ros::NodeHandle *recog_handle = EngagementParams::getRecogHandle(); ros::NodeHandle *robot_handle = EngagementParams::getRecogRobotHandle(); ros::NodeHandle *human_handle = EngagementParams::getRecogHumanHandle(); // Advertise all the services recog_statistics_srv_ = RNODE_SERVICE(recog_handle, RECOG_STATISTICS_TOPIC, &RNode::recogStatisticsService); recog_dir_gaze_srv_ = RNODE_SERVICE(robot_handle, RECOG_DIRECTED_GAZE_TOPIC, &RNode::recogDirGazeService); recog_mf_gaze_srv_ = RNODE_SERVICE(robot_handle, RECOG_MUTUAL_FACIAL_GAZE_TOPIC, &RNode::recogMFGazeService); recog_adjacency_pair_srv_ = RNODE_SERVICE(robot_handle, RECOG_ADJACENCY_PAIR_TOPIC, &RNode::recogAdjacencyPairService); // Create all the publishers for outgoing messages dealing with the human status_pub_ = RNODE_ADVERTISE(human_handle, ActorIDStatus, engagement::RECOG_STATUS_TOPIC); } void RNode::startSharedGaze() { // If the shared gaze is already started, end the shared gaze, // before starting a new one if (this->shared_gaze_start_ != UNDEFINED_TIME) this->endSharedGaze(); // The shared gaze has started this->shared_gaze_start_ = CURRENT_TIME; this->debug(9, "Shared gaze started."); } void RNode::endSharedGaze() { // Ensure that the start time is not undefined if (this->shared_gaze_start_ != UNDEFINED_TIME) { // The shared gaze has ended // Calculate the duration of shared gaze unsigned long duration = CURRENT_TIME - this->shared_gaze_start_; // Add the shared gaze duration to the overall duration this->shared_gaze_.event(duration); // Reset the shared gaze duration this->shared_gaze_start_ = UNDEFINED_TIME; this->debug(9, "Shared gaze completed [%ld].", duration); } } void RNode::relayToActors(std::string topic, ros::Message *message) { ActorManager::relayToActors(topic, message); } void RNode::castEvent(Event event) { // Grab the additional data from the event std::vector<std::string> event_additional = event.getAdditional(); // Determine which type of event we received if (EventFactory::isCompleted(event)) { // Make sure we have all the information if (event_additional.size() == 8) { // Grab the information from the event std::string type = event_additional.at(0); std::string initiator = event_additional.at(1); std::string actor_id = event_additional.at(2); std::string str_start = event_additional.at(3); std::string str_delay = event_additional.at(4); std::string str_duration = event_additional.at(5); std::string str_success = event_additional.at(6); std::string str_data = event_additional.at(7); // Attempt to find the actor Actor *actor = this->findActor(actor_id); // Ensure that this actor exists if (!actor) { this->warn("Actor [%s] does not exist.", actor_id.c_str()); this->error("Rejecting connection event event."); return; } // Determine the success and duration of the connection event bool success = (str_success == EventFactory::getSuccessName() ? true : false); // Publish the connection event message on the connection event topic this->green("CONNECTION EVENT: [%s %s %s %s %s %s %s %s]", type.c_str(), initiator.c_str(), actor_id.c_str(), str_start.c_str(), str_delay.c_str(), str_duration.c_str(), str_success.c_str(), str_data.c_str()); // Check the engaged status of this actor if (!actor->getEngaged()) { // If the actor was not engaged, we need to send an engaged message // Create the Actor message engagement_msgs::ActorIDStatus actor_msg; // The message relates to this actor, and it is true for engaged actor_msg.actor.id = actor_id; actor_msg.status.value = bml_msgs::Flag::TRUE; // Publish the engaged message externally and internally this->status_pub_.publish(actor_msg); this->relayToActors(engagement::RECOGNITION_NAMESPACE + "/" + engagement::HUMAN_NAMESPACE + "/" + engagement::RECOG_STATUS_TOPIC, &actor_msg); // Set the actor's engaged status to true actor->setEngaged(true); } // If it failed, we need to update the wait for the recognizer if (success == false) { // Grab the type of the connection event std::string ce_type = event_additional.at(0) + " Event Failed."; // Print out that the specific event failed this->info(ce_type.c_str()); } } else this->warn("CONNECTION EVENT: expected 7 pieces of data, got %d", event_additional.size()); } else if (EventFactory::isSuccess(event)) { // Make sure the event contains the type of event that succeeded if (event_additional.size() > 0) { // Grab the type of the connection event std::string ce_type = event_additional.at(0) + " Event Succeeded."; // Print out that the specific event succeeded this->info(ce_type.c_str()); } else this->error("Received [null] event type."); } } bool RNode::compareAdditional(std::vector<std::string> additional, std::string compare) { // RNode has no additional data to compare currently return false; } bool RNode::foundActor(std::string id) { // Try to locate the object Actor *found_actor = this->findActor(id); // If the object already exists we cannot add it. if (found_actor != NULL) { this->warn("Cannot add actor '%s'. It already exists!", id.c_str()); return false; } // Create a new actor using the id Actor *actor = ActorManager::createActor(id); // Create a new engagement handler, with the actor, the robot and the RNode EngagementHandler *ehandler = new EngagementHandler(id, (EventSink*)this); // Set the actor's engagement handler actor->setHandler(ehandler); this->debug(9, "Added actor [%s].", actor->getID().c_str()); // Set the start time of the logger to the current time to keep // track of the start of the interaction EngagementParams::setStartTime(CURRENT_TIME); return true; } bool RNode::lostActor(std::string id) { // Try to find the actor in the hash table Actor *act = this->findActor(id); // Make sure the actor exists if (act != NULL) { ActorManager::removeActor(id); // The object has not been updated recently enough, get rid of it this->debug(9, "Deleted actor '%s'.", id.c_str()); return true; } return false; } Actor* RNode::findActor(std::string id) { return ActorManager::findActor(id); } void *RNode::spin(void *param) { // Assign a sleep rate ros::Rate sleep_rate(10); pthread_t myself = pthread_self(); // Now, so long as we're the master we need to spin while (pthread_equal(**((pthread_t**)param), myself) != 0) { // We need to spin one time ros::spinOnce(); // Sleep for the given rate sleep_rate.sleep(); } // Exit this thread now that we're no longer the master pthread_exit((void*)true); return (void*)true; } void RNode::spawnThread() { // Add the thread to the list of old threads if (this->master_thread_ != NULL) this->old_threads_.push_back(this->master_thread_); // Create memory for the new thread pthread_t *new_thread = (pthread_t*)calloc(1, sizeof(pthread_t)); // Store the new thread as the master thread this->master_thread_ = new_thread; int status = -1; // Call the spin function with the new thread while (status != 0) { status = pthread_create(new_thread, NULL, &RNode::spin, (void*)&this->master_thread_); } } bool RNode::masterExists() { // Return true if the master thread exists, false otherwise return (this->master_thread_ != NULL); } void RNode::updateOldThreads() { // Traverse through all the old threads for (unsigned c = 0; c < this->old_threads_.size(); c++) { bool remove = false; pthread_t *thread = this->old_threads_.at(c); // Determine if the thread exists or not if (thread == NULL) remove = true; else if (pthread_tryjoin_np(*(thread), NULL) == 0) remove = true; // Remove the old thread from the list if (remove) { // Free the thread if it exists if (thread != NULL) free(thread); this->old_threads_.erase(this->old_threads_.begin() + c); c--; } } }

---

engagement_recognition
Author(s): Brett Ponsler (bponsler (at) wpi (dot) edu)
autogenerated on Fri Sep 9 10:35:48 2011