/*
 * util.hpp
 *
 *  Created on: 24/06/2013
 *      Author: timothyw
 */

#ifndef VICTIMID_UTIL_HPP_
#define VICTIMID_UTIL_HPP_

#include "class.hpp"

// Common defines
#define EXPLORER_MODE_RIGHT_WALL_STR        "right"
#define EXPLORER_MODE_LEFT_WALL_STR         "left"
#define EXPLORER_MODE_PAUSED                0
#define EXPLORER_MODE_RESUME                1
#define EXPLORER_MODE_LEFT_WALL             2
#define EXPLORER_MODE_RIGHT_WALL            3
#define EMU_ARM_LOOKAT_MODE_STOP            0
#define EMU_ARM_LOOKAT_MODE_START           1

using namespace crosbot;

namespace casualty {

class VIDUtil {
public:
    // Transform the given 3D point
    static Point3D transformPoint(tf::TransformListener* tfListener, std::string fromFrame, std::string toFrame,
            ros::Time timestamp, Point3D p);
    static Pose transformPose(tf::TransformListener* tfListener, std::string fromFrame, std::string toFrame,
                ros::Time timestamp, Pose pose);

    // Retrieve the required transform
    static tf::StampedTransform getTransform(tf::TransformListener* tfListener, std::string fromFrame,
            std::string toFrame, ros::Time timestamp);
};

} // namespace casualty


#endif /* VICTIMID_UTIL_HPP_ */
