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

engagement_recognition/src/recognition/recognizers/directed_gaze/directed_gaze_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 "directed_gaze_recognizer.h" #include "../../events/event_factory.h" #include "../../lib/engagement_params.h" #include "../../lib/engagement_macros.h" #include "../../objects/engagement_objects.h" DirectedGazeRecognizer::DirectedGazeRecognizer() : Recognizer(DirectedGazeRecognizer::type(), NULL) { // Set the logger based on this type of recognizer this->setLogger(EngagementParams::getDGTag("")); // Load the max delay parameter this->getMaxDelayParam(EngagementParams::getConfDirGazeHandle(), 2.0); // Cast ourselves into a Recognizer Recognizer *recognizer = (Recognizer*)this; // Create the start state, and set the state to it recognizer->setStartState(); this->object_ = std::string(""); // Grab the recognition/human NodeHandle ros::NodeHandle *recog_human = EngagementParams::getRecogHumanHandle(); // Advertise the human initiated directed gaze topic hi_dir_gaze_pub_ = recog_human->advertise<engagement_msgs::HumanDirectedGaze>( engagement::RECOG_DIRECTED_GAZE_TOPIC, MAX_BUFFER); this->warn("Initialized recognizer to an empty actor, and object."); } DirectedGazeRecognizer::DirectedGazeRecognizer(std::string actor, std::string object, EventSink *sink) : Recognizer(DirectedGazeRecognizer::type(), sink) { // Set the logger based on this type of recognizer this->setLogger(sink->getLogger(), EngagementParams::getDGTag(object)); // Load the max delay parameter this->getMaxDelayParam(EngagementParams::getConfDirGazeHandle(), 2.0); // Set the actor for this recognizer // Do this BEFORE setting the state setActor(actor); this->object_ = std::string(object); // Grab the recognition/human NodeHandle ros::NodeHandle *recog_human = EngagementParams::getRecogHumanHandle(); // Advertise the human initiated directed gaze topic hi_dir_gaze_pub_ = recog_human->advertise<engagement_msgs::HumanDirectedGaze>( engagement::RECOG_DIRECTED_GAZE_TOPIC, MAX_BUFFER); // Create the start state and set the state to it this->setStartState(); } void DirectedGazeRecognizer::initialize(initiator::Initiator initiator) { Recognizer *rec = (Recognizer*)this; // If the human was the initiator, send a human initiated message if (initiator == initiator::HUMAN) { // Grab the internal node handle ros::NodeHandle *n = EngagementParams::getConfDirGazeHandle(); // Grab the max delay from the parameter server double max_delay = this->getMaxDelayParam(n, 2.0); // Only send human initiated events if the recognizer is not degraded if (!EngagementParams::isDegraded()) { // Create the human initiated directed gaze message engagement_msgs::HumanDirectedGaze dg_msg; // Set the actor, the timeout duration, and the objects for this message dg_msg.actor.id = this->getActor()->getID(); dg_msg.timeout = max_delay; dg_msg.objects = std::vector<bml_msgs::Entity>(); dg_msg.objects.push_back(objects::objectToEntity(this->object_)); dg_msg.action = this->action_.toValue(); this->debug(5, "Publishing human initiated event for action [%s].", this->action_.toString()); // Publish the message this->hi_dir_gaze_pub_.publish(dg_msg); } } // Initialize this recognizer with the initiator rec->initialize(initiator); } void DirectedGazeRecognizer::transition(Event event) { std::vector<std::string> additional = event.getAdditional(); // Check the event cases if (EventFactory::isRobotInitiated(event)) { this->debug(8, "ROBOT INITIATED EVENT!"); // Unblock the semaphore and enter the robot directing state this->unblock(ServiceResponse::cycle(), Action::noAction()); this->setState(factory::DG_ROBOT_DIRECTING); // The robot directs gaze with points this->action_ = Action::point(); this->action_.setData(this->object_); // Initialize the connection event this->initialize(initiator::ROBOT); this->start_delay_ = CURRENT_TIME; } else if (EventFactory::isHumanInitiated(event)) { this->debug(8, "HUMAN INITIATED EVENT"); // If we're currently in the robot directing state, then we fail if (this->getStateName() == StateFactory::getType(factory::DG_ROBOT_DIRECTING)) { // The connection event failed this->complete(false); } // Unblock the semaphore and enter the human directing state this->unblock(ServiceResponse::cycle(), Action::noAction()); this->setState(factory::DG_HUMAN_DIRECTING); if (additional.size() > 0) { this->action_ = Action::fromString(additional.at(0)); this->action_.setData(this->object_); } // Initialize the connection event this->initialize(initiator::HUMAN); this->start_delay_ = CURRENT_TIME; } else if (EventFactory::isSharedGaze(event)) { this->debug(8, "SHARED GAZE EVENT"); // Move to the shared gaze state this->setState(factory::DG_SHARED_GAZE); // We have succeeded (look always completes DG) this->succeed(Action::look(this->object_)); this->end_delay_ = CURRENT_TIME; } else if (EventFactory::isCycle(event)) { this->debug(8, "CYCLE EVENT"); // Unblock the semaphore so it re-blocks this->unblock(ServiceResponse::cycle(), Action::noAction()); // Reset the start delay this->start_delay_ = CURRENT_TIME; } else if (EventFactory::isSuccess(event)) { this->debug(8, "SUCCESS EVENT"); // Transition to the start state unless we're already there, which // we then move to the shared gaze state if (this->getStateName() != StateFactory::getType(factory::DG_START)) { // Move to the start state this->setStartState(); // Complete the connection event successfully this->complete(true); } else { // Move to the shared gaze state this->setState(factory::DG_SHARED_GAZE); // Set the start time, and end and start delay // (so we have a delay of zero) this->start_time_ = CURRENT_TIME; this->start_delay_ = this->start_time_; this->end_delay_ = this->start_delay_; // We have succeeded (look always completes DG) this->succeed(Action::look(this->object_)); } } else if (EventFactory::isFailed(event)) { this->debug(8, "FAILED EVENT"); // Move to the start state this->setStartState(); // Complete the connection event unsuccessfully this->complete(false); } } std::string DirectedGazeRecognizer::getObject() { return this->object_; } std::string DirectedGazeRecognizer::type() { return engagement::DIRECTED_GAZE_TYPE; }

---

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