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

engagement_recognition/src/recognition/actors/robot.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 "robot.h" #include <ros/ros.h> #include "../objects/engagement_objects.h" #include "../lib/engagement_params.h" Robot *Robot::instance_ = NULL; Robot::Robot() { // Create the prefix for the logger std::string prefix = std::string(EngagementParams::getRecogTag()) + std::string(EngagementParams::getActorTag(engagement::ROBOT_TYPE)); // Create the logger for the robot class this->setLogger(prefix); this->createSemaphores(); // Initiate the values this->speaking_ = false; this->facial_gaze_ = std::string(""); this->looking_ = std::vector<std::string>(); this->pointing_ = std::vector<std::string>(); this->waiting_ = false; } void Robot::destroy() { if (Robot::instance_ != NULL) { // First delete any semaphores delete Robot::instance_->semaphore_; delete Robot::instance_; Robot::instance_ = NULL; } } Robot* Robot::getRobot() { if (Robot::instance_ == NULL) Robot::instance_ = new Robot(); return Robot::instance_; } void Robot::createSemaphores() { this->semaphore_ = new Semaphore(1); } bool Robot::isSpeaking() { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); bool temp = inst->speaking_; inst->semaphore_->post(); return temp; } bool Robot::isFacialGaze(std::string actor) { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); std::string temp = inst->facial_gaze_; inst->semaphore_->post(); return (temp == actor); } std::vector<std::string> Robot::getLooking() { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); std::vector<std::string> temp = inst->looking_; inst->semaphore_->post(); return temp; } std::vector<std::string> Robot::getPointing() { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); std::vector<std::string> temp = inst->pointing_; inst->semaphore_->post(); return temp; } bool Robot::isLooking(std::vector<std::string> objects) { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); std::vector<std::string> temp = inst->looking_; inst->semaphore_->post(); // Make sure the robot is looking somewhere // Then, return if the set of objects are identical if (temp.size() > 0) return objects::same(temp, objects); return false; } bool Robot::isPointing(std::vector<std::string> objects) { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); std::vector<std::string> temp = inst->pointing_; inst->semaphore_->post(); // Make sure the robot is pointing somewhere // Then, return if the set of objects are identical if (temp.size() > 0) return objects::same(temp, objects); return false; } void Robot::setSpeaking(bool start) { Robot *inst = Robot::getRobot(); // Set the robot's speaking flag accordingly inst->semaphore_->wait(); inst->speaking_ = start; inst->semaphore_->post(); } void Robot::setFacialGaze(bool start, std::string actor) { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); std::string temp = inst->facial_gaze_; inst->semaphore_->post(); if (start && actor != NO_FACIAL_GAZE) { inst->semaphore_->wait(); inst->facial_gaze_ = std::string(actor); // Clear the vector of objects the robot is looking at inst->looking_.clear(); inst->semaphore_->post(); inst->debug(9, "Robot is making facial gaze with [%s].", actor.c_str()); } else if (temp != NO_FACIAL_GAZE) { inst->debug(9, "Robot stopped making facial gaze with [%s].", inst->facial_gaze_.c_str()); // The robot is no longer making facial gaze with an actor inst->semaphore_->wait(); inst->facial_gaze_ = std::string(NO_FACIAL_GAZE); inst->semaphore_->post(); } } void Robot::setLookAt(bool start, std::vector<std::string> objects) { Robot *inst = Robot::getRobot(); if (start) { // Set the vector of objects the robot is looking at to // the new objects inst->semaphore_->wait(); inst->looking_ = std::vector<std::string>(objects); std::string temp = inst->facial_gaze_; inst->semaphore_->post(); // The robot is not making facial gaze anymore inst->setFacialGaze(false, temp); // Create meaningful output std::stringstream out; for (unsigned i = 0; i < objects.size(); i++) { // Only add a comma if it's not the first element if (i != 0) out << "," << objects.at(i); else out << objects.at(i); } // Display the objects the robot is looking at inst->debug(9, "Robot started looking at [%s].", out.str().c_str()); } else { // Create meaningful output std::stringstream out; inst->semaphore_->wait(); std::vector<std::string> temp = inst->looking_; inst->semaphore_->post(); // Traverse the incoming objects for (unsigned i = 0; i < objects.size(); i++) { // Traverse all the objects we're looking at for (unsigned c = 0; c < temp.size(); c++) { // If the object matches remove it from the vector if (temp.at(c) == objects.at(i)) { temp.erase(inst->looking_.begin() + c); break; } } inst->semaphore_->wait(); inst->looking_ = temp; inst->semaphore_->post(); // Only add a comma if it's not the first element if (i != 0) out << "," << objects.at(i); else out << objects.at(i); } // Display the objects the robot is looking at inst->debug(9, "Robot stopped looking at [%s].", out.str().c_str()); } } void Robot::setPointAt(bool start, std::vector<std::string> objects) { Robot *inst = Robot::getRobot(); // If the pointing is starting if (start) { inst->semaphore_->wait(); std::vector<std::string> temp = inst->pointing_; inst->semaphore_->post(); // Create meaningful output std::stringstream out; for (unsigned i = 0; i < objects.size(); i++) { bool found = false; // Traverse through the objects currently being looked at for (unsigned c = 0; c < temp.size(); c++) { // Make sure the new objects don't already exist if (objects.at(i) == temp.at(c)) { found = true; break; } } // Only add objects that have not been found yet if (!found) { // Only add a comma if it's not the first element if (i != 0) out << "," << objects.at(i); else out << objects.at(i); // The robot is now pointing at inst object temp.push_back(objects.at(i)); } } inst->semaphore_->wait(); inst->pointing_ = temp; inst->semaphore_->post(); // Display the objects the robot is looking at and which arm is being used inst->debug(9, "Robot started pointing at [%s]", out.str().c_str()); } else { // The robot has stopped pointing at objects // Create meaningful output std::stringstream out; inst->semaphore_->wait(); std::vector<std::string> temp = inst->pointing_; inst->semaphore_->post(); // Traverse the incoming objects for (unsigned i = 0; i < objects.size(); i++) { // Traverse all the objects we're pointing at for (unsigned c = 0; c < temp.size(); c++) { // If the object matches remove it from the vector if (temp.at(c) == objects.at(i)) { temp.erase(temp.begin() + c); break; } } inst->semaphore_->wait(); inst->pointing_ = temp; inst->semaphore_->post(); // Only add a comma if it's not the first element if (i != 0) out << "," << objects.at(i); else out << objects.at(i); } // Display the objects the robot is looking at inst->debug(9, "Robot stopped pointing at [%s].", out.str().c_str()); } } void Robot::donePoint() { Robot *inst = Robot::getRobot(); // Clear all the objects out of the vector depending on the arm inst->semaphore_->wait(); inst->pointing_.clear(); inst->semaphore_->post(); inst->debug(9, "Robot is no longer pointing at any objects."); } bool Robot::isWaiting() { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); bool temp = inst->waiting_; inst->semaphore_->post(); return temp; } void Robot::setWaiting(bool state) { Robot *inst = Robot::getRobot(); inst->semaphore_->wait(); bool temp = inst->waiting_; inst->semaphore_->post(); // Make sure the state is different if (temp != state) { inst->semaphore_->wait(); inst->waiting_ = state; inst->semaphore_->post(); } inst->debug(10, "Set robot's waiting state to [%s]", (state ? "true" : "false")); }

---

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