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

engagement_recognition/src/recognition/recognizers/recognizer.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 "recognizer.h" #include "../logger/file_logger.h" #include "../events/event_factory.h" #include "../lib/engagement_params.h" #include "../lib/engagement_macros.h" #include "../actors/actor_manager.h" Recognizer::~Recognizer() { // Delete this state if (this->state_) delete this->state_; // Delete the semaphores delete this->state_semaphore_; delete this->semaphore_; } Recognizer::Recognizer() : EventSink() { // Have no state this->state_ = NULL; // Set the start and end time to undefined this->start_time_ = UNDEFINED_TIME; this->end_time_ = UNDEFINED_TIME; // Set the start and end delay to undefined this->start_delay_ = UNDEFINED_TIME; this->end_delay_ = UNDEFINED_TIME; // Start the max timeout at -1 this->max_delay_ = -1; // Have no succeeded yet this->succeeded_ = false; // Has not been initiated yet this->initiator_ = initiator::NO_INIT; // No type identifier given this->type_ = ""; // Do not have an actor yet this->actor_ = ""; // Create the semaphores this->semaphore_ = new Semaphore(1); this->semaphore_ = new Semaphore(0); } Recognizer::Recognizer(std::string type, EventSink *sink) : EventSink(sink) { // Have no state this->state_ = NULL; // Set the start and end time to undefined this->start_time_ = UNDEFINED_TIME; this->end_time_ = UNDEFINED_TIME; // Set the start and end delay to undefined this->start_delay_ = UNDEFINED_TIME; this->end_delay_ = UNDEFINED_TIME; // Start the max timeout at -1 this->max_delay_ = -1; // Have no succeeded yet this->succeeded_ = false; // Has not been initated yet this->initiator_ = initiator::NO_INIT; // Use the given type this->type_ = type; // Do not have an actor yet this->actor_ = ""; // Create the semaphores this->state_semaphore_ = new Semaphore(1); this->semaphore_ = new Semaphore(0); } Recognizer::Recognizer(State *state, std::string type, std::string actor, EventSink *sink) : EventSink(sink) { // Begin in the start state this->state_ = state; // Set the start and end time to undefined this->start_time_ = UNDEFINED_TIME; this->end_time_ = UNDEFINED_TIME; // Set the start and end delay to undefined this->start_delay_ = UNDEFINED_TIME; this->end_delay_ = UNDEFINED_TIME; // Start the max timeout at -1 this->max_delay_ = -1; // Have not succeeded yet this->succeeded_ = false; // Has not been initated yet this->initiator_ = initiator::NO_INIT; // Use the given type this->type_ = type; // Use the given actor this->actor_ = actor; // Create the semaphores this->state_semaphore_ = new Semaphore(1); this->semaphore_ = new Semaphore(0); } void Recognizer::received(std::string topic, ros::Message *message) { // Create an empty event, which will help us determine // whether an event was created and needs to be handled this->state_semaphore_->wait(); Event event = this->state_->received(topic, message); this->state_semaphore_->post(); // Make sure the event exists, and then use it to transition if (event.getName().length() != 0) this->transition(event); } void Recognizer::callService(boost::barrier *barrier) { // Set the status to cycling this->status_.setCycle(); // Block, until the even completes this->block(); // Tell the thread barrier we're done processing if (barrier) barrier->wait(); } void Recognizer::castEvent(Event event) { // Call the transition method which will be // implemented by concrete recognizers this->transition(event); } void Recognizer::setState(factory::StateType type) { // Cast ourselves into an EventSink EventSink *sink = (EventSink*)this; // Create the new state State *new_state = StateFactory::createState(type, this->actor_, sink); // Grab the state temporarily and the name this->state_semaphore_->wait(); State *temp = this->state_; const char *temp_name = (temp != NULL ? this->state_->getName() : ""); this->state_semaphore_->post(); // Display output based on whether we're currently in a state or not if (temp != NULL) { // Determine if the new state exists if (new_state) { this->debug(5, "\033[96mrecognizer: [%s] -> [%s].\033[0m", temp_name, new_state->getName()); } else { this->error("\033[96mrecognizer: -> Invalid state change.\033[0m"); } } else if (new_state) { this->debug(5, "\033[96mrecognizer: -> [%s].\033[0m", new_state->getName()); } // Only change states if it was a valid transition if (new_state) { // Make sure to block with the semaphore this->state_semaphore_->wait(); // Free the memory for the current state prior to switching states if (this->state_) delete this->state_; // Set the state to the new state this->state_ = new_state; this->state_semaphore_->post(); } } void Recognizer::setStartState() { // Get the start state type based on what type of recognizer this is factory::StateType type = StateFactory::getStartState(this->type_); // Make sure the type was found, and set the state to the new start state if (type != factory::INVALID_STATE) this->setState(type); } const char *Recognizer::getStateName() { this->state_semaphore_->wait(); const char *name = this->state_->getName(); this->state_semaphore_->post(); return name; } bool Recognizer::compareAdditional(std::vector<std::string> additional, std::string compare) { //NOTE: Same means that the additional contains the objects // different means that the additional does not contain the objects // Make sure there are additional objects if (additional.size() > 0) { // Traverse through all of the additional objects for (unsigned d = 0; d < additional.size(); d++) { // Determine if the objects are the same if (this->getObject() == additional.at(d)) { // If we're looking for the same, then true, if different then false if (compare == EventSink::CONTAINS) return true; } } // The object was not found in the list if (compare == EventSink::CONTAINS) return false; this->warn("Received an unrecognized comparison request '%s'.", compare.c_str()); } return false; } void Recognizer::succeed(Action action) { this->succeeded_ = true; // Make sure we have a parent event sink if (this->parent_) { // Create the additional information, with the type of this recognizer std::vector<std::string> additional = std::vector<std::string>(); additional.push_back(this->type_); // Unblock with the succeeded case this->unblock(ServiceResponse::succeeded(), action); // Cast the success or failure event upward using the additional data this->parent_->castEvent(EventFactory::success(additional)); } } void Recognizer::initialize(initiator::Initiator initiator) { // Set the start time for this recognizer this->start_time_ = CURRENT_TIME; // Store the initiator this->initiator_ = initiator; // Make sure the actor field has been set if (this->actor_ == "") this->warn("[%s] recognizer: actor field has not been set.", this->type_.c_str()); } void Recognizer::complete(bool success) { // Set the success and end time for this connection event this->succeeded_ = success; this->end_time_ = CURRENT_TIME; // If this is a failed event, we need to unblock the mutex if (!success) { // Unblock with the failed case, and no action this->unblock(ServiceResponse::failed(), Action::noAction()); } // Convert the start time to a string stream std::stringstream start; start << this->start_time_; // Convert the duration into a string stream std::stringstream duration; // Select the duration based on if it undefined or not if (this->start_time_!= UNDEFINED_TIME && this->end_time_ != UNDEFINED_TIME) { // Calculate the duration and add it to the stringstream unsigned long total_duration = (this->end_time_ - this->start_time_); duration << total_duration; } else duration << 0; // Convert the delay into a string stream std::stringstream delay; // Select the delay time based on if it undefined or not if (this->start_delay_ != UNDEFINED_TIME && this->end_delay_ != UNDEFINED_TIME) { // Calculate the delay and add it to the stringstream unsigned long total_delay = (this->end_delay_ - this->start_delay_); delay << total_delay; } else delay << 0; // Grab the name of the file to log to from the param server std::string log_file = EngagementParams::getLogFile(); // Print out the completion of the event to the CSV file as: // start time, type, delay, duration, initiator, success FileLogger::log(log_file.c_str(), "%ld,%s,%s,%s,%s,%s,%s\n", this->start_time_ - EngagementParams::getStartTime(), this->type_.c_str(), delay.str().c_str(), duration.str().c_str(), (this->initiator_ == initiator::ROBOT ? "ROBOT" : this->actor_.c_str()), (this->succeeded_ ? "SUCCESS" : "FAILURE"), this->getObject().c_str()); // Add the information to the vector for additional data std::vector<std::string> additional = std::vector<std::string>(); additional.push_back(this->type_); additional.push_back((this->initiator_ == initiator::ROBOT ? "ROBOT" : "HUMAN")); additional.push_back(this->actor_); additional.push_back(start.str()); additional.push_back(delay.str()); additional.push_back(duration.str()); additional.push_back((this->succeeded_ ? "SUCCESS" : "FAILURE")); additional.push_back(this->getObject()); // Make sure we have a parent event sink and then // cast the success or failure event upward using the additional data if (this->parent_ != NULL) this->parent_->castEvent(EventFactory::completed(additional)); // Reset all the values and times this->succeeded_ = false; this->initiator_ = initiator::NO_INIT; this->start_time_ = UNDEFINED_TIME; this->end_time_ = UNDEFINED_TIME; this->start_delay_ = UNDEFINED_TIME; this->end_delay_ = UNDEFINED_TIME; } void Recognizer::setActor(std::string actor) { if (actor == "") this->warn("Setting [%s] recognizer to an empty actor.", this->type_.c_str()); this->actor_ = actor; } Actor *Recognizer::getActor() { return ActorManager::findActor(this->actor_); } std::string Recognizer::getObject() { return std::string(""); } ServiceResponse Recognizer::getStatus() { return this->status_; } double Recognizer::getMaxDelayParam(ros::NodeHandle *handle, double def) { // Only load the parameter if it has not been loaded yet if (this->max_delay_ == -1) { // Make sure the handle exists, and try to grab the parameter // use a default of -1 if (handle) handle->param(engagement::MAX_DELAY_PARAM, this->max_delay_, def); else this->max_delay_ = def; return this->max_delay_; } return def; } void Recognizer::setAction(Action action) { this->action_ = action; } Action Recognizer::getAction() { return this->action_; } void Recognizer::block() { // Wait on the semaphore until it reaches a value of zero while (this->semaphore_->getValue() > 0) this->semaphore_->wait(); // Block until we are finished cycling do { double delay = this->max_delay_; this->debug(15, "Service semaphore blocking."); // If degraded, select a random timeout between 0 and 2*normal timeout if (EngagementParams::isDegraded()) { if (rand() % 2 == 0) delay = .01; else delay = 2 * this->max_delay_; this->cyan("Degraded selected timeout of [%lf]", delay); } // Block the semaphore for the given amount of time, if the // wait fails (ends due to duration) then it failed if (!this->semaphore_->timedWait(delay)) { int verb = (EngagementParams::isDegraded() ? 5 : 15); this->debug(verb, "Semaphore timed out and unblocked itself."); // Set the status to failed this->status_.setFailed(); // Set this state to the start state this->setStartState(); // Complete the recognizer with the false case this->complete(false); } this->debug(15, "Service semaphore unblocked."); } while (this->status_.isCycle()); this->debug(15, "Service semaphore unblocked completely."); } void Recognizer::unblock(ServiceResponse status, Action action) { // The recognizer cannot unblock itself if it's degraded if (EngagementParams::isDegraded()) return; // Set our status and action to the given values this->status_ = status; this->action_ = action; // Only post if the semaphore is blocking if (this->semaphore_->getValue() <= 0) { this->debug(15, "Posting the service semaphore."); this->semaphore_->post(); } }

---

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