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

engagement_recognition/src/recognition/rnode/callbacks/vision.h 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) #ifndef _VISION_H #define _VISION_H void RNode::visionFacialGazeCallback(const engagement_msgs::PerformanceConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_FACIAL_GAZE_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Determine if this message was rejected or not bool rejected = false; // Grab the message type from the constant pointer engagement_msgs::Performance temp_msg = *msg; // Attempt to locate the actor Actor *actor = this->findActor(temp_msg.actor.id); // Make sure the actor exists if (!actor) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); rejected = true; } // Now make sure the actor is not currently in the // same state as the message if (!rejected && actor->isFacialGaze() == (temp_msg.begin.value == bml_msgs::Flag::BEGIN)) { this->warn("Actor [%s] is %s in facial gaze state.", temp_msg.actor.id.c_str(), (temp_msg.begin.value == bml_msgs::Flag::BEGIN ? "already" : "not")); rejected = true; } // Determine if this message was rejected. if (rejected) { this->error("Rejected '%s' message.", engagement::PERFORMING_FACIAL_GAZE_TOPIC.c_str()); return; } this->debug(6, "Received a %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_FACIAL_GAZE_TOPIC.c_str()); // Update the facial gaze state based on the received message actor->setFacialGaze((temp_msg.begin.value == bml_msgs::Flag::BEGIN)); // If the facial gaze is starting, and the robot is in // facial gaze, shared gaze starts // If the facial gaze is ending, and the robot is in // facial gaze, shared gaze ends if (temp_msg.begin.value == bml_msgs::Flag::BEGIN && Robot::isFacialGaze(temp_msg.actor.id)) this->startSharedGaze(); else if (Robot::isFacialGaze(temp_msg.actor.id) && this->shared_gaze_start_ != UNDEFINED_TIME) this->endSharedGaze(); // Publish the message internally this->relayToActors(engagement::VISION_NAMESPACE + "/" + engagement::PERFORMING_FACIAL_GAZE_TOPIC, &temp_msg); } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_FACIAL_GAZE_TOPIC.c_str()); } void RNode::visionLookCallback(const engagement_msgs::PerformanceConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_LOOK_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Determine if the message was rejected or not bool rejected = false; // Grab the message type from the constant pointer engagement_msgs::Performance temp_msg = *msg; // Keep the objects with the highest probability objects::keepHighest(temp_msg); // Grab the vector of objects from the vector of entities std::vector<std::string> msg_objects = objects::getObjectsFromEntities(temp_msg.objects); // Attempt to locate the actor Actor *actor = this->findActor(temp_msg.actor.id); // Make sure the actor exists if (!actor) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); rejected = true; } // Determine if the look is starting or ending if (!rejected && temp_msg.begin.value) { // Check to see if the actor is already looking at these objects if (objects::contains(actor->getLooking(), msg_objects)) { this->warn("Actor [%s] is already looking at objects.", temp_msg.actor.id.c_str()); rejected = true; } } else if (!rejected && !temp_msg.begin.value) { // Check to see if the actor is not already looking at these objects if (!objects::contains(actor->getLooking(), msg_objects)) { this->warn("Actor [%s] is not looking at objects.", temp_msg.actor.id.c_str()); rejected = true; } } // Check if the message was rejected if (rejected) { this->error("Rejected '%s' message.", engagement::PERFORMING_LOOK_TOPIC.c_str()); return; } // Ensure that the actor is still alive if (ActorManager::isAlive(temp_msg.actor.id)) { this->debug(6, "Received a %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_LOOK_TOPIC.c_str()); // Update the actor's recognizers with all of the objects actor->updateDirGazeRecognizers(msg_objects); // Update the looking with the object with the highest probability actor->setLooking(msg_objects, temp_msg.begin.value); // If the actor and robot were in facial gaze then end the shared gaze if (this->shared_gaze_start_ != UNDEFINED_TIME) this->endSharedGaze(); // Publish the message internally this->relayToActors(engagement::VISION_NAMESPACE + "/" + engagement::PERFORMING_LOOK_TOPIC, &temp_msg); } } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_LOOK_TOPIC.c_str()); } void RNode::visionPointCallback(const engagement_msgs::PerformanceConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_POINT_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Determine if the message was rejected or not bool rejected = false; // Grab the message type from the constant pointer engagement_msgs::Performance temp_msg = *msg; // Keep the objects with the highest probability objects::keepHighest(temp_msg); // Grab the vector of objects from the vector of entities std::vector<std::string> msg_objects = objects::getObjectsFromEntities(temp_msg.objects); // Attempt to locate the actor Actor *actor = this->findActor(temp_msg.actor.id); // Make sure the actor exists if (!actor) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); rejected = true; } // Make sure we're not yet rejected if (!rejected) { // Determine if the actor is currently looking at the same objects bool contained = objects::contains(actor->getPointing(), msg_objects); // If we're starting the look, the actor cannot be looking at the objects // If we're ending the look, the actor must be looking at the objects if (temp_msg.begin.value && contained) { this->warn("Actor [%s] is already pointing at objects.", temp_msg.actor.id.c_str()); rejected = true; } else if (!temp_msg.begin.value && !contained) { this->warn("Actor [%s] is not pointing at objects.", temp_msg.actor.id.c_str()); rejected = true; } } // Check if the message was rejected if (rejected) { this->error("Rejected '%s' message.", engagement::PERFORMING_POINT_TOPIC.c_str()); return; } this->debug(6, "Received a %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_POINT_TOPIC.c_str()); // Make sure the actor is still alive if (ActorManager::isAlive(temp_msg.actor.id)) { // Update the actor's recognizers with all of the objects actor->updateDirGazeRecognizers(msg_objects); // Update the pointing with the object with the highest probability actor->setPointing(msg_objects, temp_msg.begin.value); // Publish the message internally this->relayToActors(engagement::VISION_NAMESPACE + "/" + engagement::PERFORMING_POINT_TOPIC, &temp_msg); } } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_POINT_TOPIC.c_str()); } void RNode::visionShakeCallback(const engagement_msgs::PerformanceConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_SHAKE_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Grab the message type from the constant pointer engagement_msgs::Performance temp_msg = *msg; // Make sure the actor exists if (!this->findActor(temp_msg.actor.id)) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); this->error("Rejected '%s' message.", engagement::PERFORMING_SHAKE_TOPIC.c_str()); return; } this->debug(6, "Received a %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_SHAKE_TOPIC.c_str()); // Publish the message internally this->relayToActors(engagement::VISION_NAMESPACE + "/" + engagement::PERFORMING_SHAKE_TOPIC, &temp_msg); } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_SHAKE_TOPIC.c_str()); } void RNode::visionNodCallback(const engagement_msgs::PerformanceConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_NOD_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Grab the message type from the constant pointer engagement_msgs::Performance temp_msg = *msg; // Make sure the actor exists if (!this->findActor(temp_msg.actor.id)) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); this->error("Rejected '%s' message.", engagement::PERFORMING_NOD_TOPIC.c_str()); return; } this->debug(6, "Received a %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_NOD_TOPIC.c_str()); // Publish the message internally this->relayToActors(engagement::VISION_NAMESPACE + "/" + engagement::PERFORMING_NOD_TOPIC, &temp_msg); } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_NOD_TOPIC.c_str()); } void RNode::visionExtendedActionCallback(const engagement_msgs::PerformanceConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_EXTENDED_ACTION_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Grab the message type from the constant pointer engagement_msgs::Performance temp_msg = *msg; // Make sure the actor exists if (!this->findActor(temp_msg.actor.id)) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); this->error("Rejected '%s' message.", engagement::PERFORMING_EXTENDED_ACTION_TOPIC.c_str()); return; } this->debug(6, "Received a %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_EXTENDED_ACTION_TOPIC.c_str()); // Publish the message internally this->relayToActors(engagement::VISION_NAMESPACE + "/" + engagement::PERFORMING_EXTENDED_ACTION_TOPIC, &temp_msg); } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::PERFORMING_EXTENDED_ACTION_TOPIC.c_str()); } void RNode::visionFaceCallback(const engagement_msgs::ActorIDStatusConstPtr& msg) { this->debug(24, "Entered %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::VISION_FACE_TOPIC.c_str()); // Make sure we have a valid message if (msg) { // Determine if this message was rejected or not bool rejected = false; // Grab the message type from the constant pointer engagement_msgs::ActorIDStatus temp_msg = *msg; Actor *act = this->findActor(temp_msg.actor.id); if (temp_msg.status.value == bml_msgs::Flag::TRUE && act) { this->warn("Actor [%s] already exists.", temp_msg.actor.id.c_str()); rejected = true; } else if (temp_msg.status.value != bml_msgs::Flag::TRUE && !act) { this->warn("Actor [%s] does not exist.", temp_msg.actor.id.c_str()); rejected = true; } // Check if the message was rejected if (rejected) { this->error("Rejected '%s' message.", engagement::VISION_FACE_TOPIC.c_str()); return; } // Update the face based on the message received if (temp_msg.status.value == bml_msgs::Flag::TRUE) { this->debug(6, "Received a found %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::VISION_FACE_TOPIC.c_str()); // We found the actor this->foundActor(temp_msg.actor.id); // Publish an Unengaged status message, since the actor // is originally unengaged engagement_msgs::ActorIDStatus actor_msg; // Set the actor's id, and flag to false (for unengaged) actor_msg.actor = temp_msg.actor; actor_msg.status.value = bml_msgs::Flag::FALSE; // Publish the engaged message, externally this->status_pub_.publish(actor_msg); this->relayToActors(engagement::RECOGNITION_NAMESPACE + "/" + engagement::HUMAN_NAMESPACE + "/" + engagement::RECOG_STATUS_TOPIC, &actor_msg); // Propagate the message internally ros::spinOnce(); } else { this->debug(6, "Received a lost %s %s message.", engagement::VISION_NAMESPACE.c_str(), engagement::VISION_FACE_TOPIC.c_str()); this->lostActor(temp_msg.actor.id); // If we lost the actor we need to send the unengaged message // Create the Actor message engagement_msgs::ActorIDStatus actor_msg;; // Set the message to be for this actor, and for unengaged actor_msg.actor = temp_msg.actor; actor_msg.status.value = bml_msgs::Flag::FALSE; // Publish the unengaged message externally this->status_pub_.publish(actor_msg); this->relayToActors(engagement::RECOGNITION_NAMESPACE + "/" + engagement::HUMAN_NAMESPACE + "/" + engagement::RECOG_STATUS_TOPIC, &actor_msg); } } this->debug(24, "Exited %s %s callback", engagement::VISION_NAMESPACE.c_str(), engagement::VISION_FACE_TOPIC.c_str()); } #endif // _VISION_H

---

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