/*
 * explore.cpp
 *
 *  Created on: 17/06/2013
 *      Author: timothyw
 */

#include "victim_id/skills/skillLookAt.hpp"

#include <emu_arm/LookAt.h>

namespace casualty {

void VIDSkillLookAt::start() {
    // Load point
    geometry_msgs::PointStamped pointStamped;
    pointStamped.point = point.toROS();
    pointStamped.header.frame_id = frame;

    // Service call - only returns when the lookat is done
    emu_arm::LookAtRequest request;
    emu_arm::LookAtResponse response;
    request.point = pointStamped;
    lookAtClient->call(request, response);

    rerun = true;
}

void VIDSkillLookAt::stop() {
    // Stop does nothing
}

bool VIDSkillLookAt::equals(VicIdSkill& skOther) {
    VIDSkillLookAt* other = dynamic_cast<VIDSkillLookAt*> (&skOther);

    if (other != NULL) {
        bool retVal = true;
        retVal = retVal && other->point == point;
        return retVal;
    }

    return false;
}

} // namespace casualty
