/*
 * paused.hpp
 *
 *  Created on: 16/06/2013
 *      Author: timothyw
 */

#ifndef VICTIMID_STATE_DRIVE_TO_BLOB_HPP_
#define VICTIMID_STATE_DRIVE_TO_BLOB_HPP_

#include "victim_id/state.hpp"

#include <std_msgs/String.h>
#include "victim_id/skills/skillDriveTo.hpp"
#include "victim_id/skills/skillLookTrack.hpp"

#define VID_STATE_DRIVE_TO_BLOB    "state_drive_to_blob"

namespace casualty {

class VIDSDriveToBlob : public VIDState {
public:
    VIDSDriveToBlob();
    ~VIDSDriveToBlob() {};

    void init(ros::NodeHandle& nh, VIDConfigPtr config);

protected:
    VIDSReturnDataPtr doExecute(VIDDataPtr data);

    // ROS Subscriptions
    ros::Subscriber explorerSub;
    void callbackExplorer(const std_msgs::String& response);

    Point3D driveToPos;         // In Robot position
    std::string driveToFrame;   // Frame of drive to
    bool nfRobotBlocked;        // Driving robot got blocked
    bool nfRobotArrive;         // Driving robot arrived
    ros::Time start;            // Time starting driving

    // Last skills (if we are continuing)
    VIDSkillDriveToPtr lastDriveTo;
    VIDSkillLookTrackPtr lastLookTrack;
};

} // namespace casualty

#endif /* VICTIMID_STATE_DRIVE_TO_BLOB_HPP_ */
