recorded_objects_transformer.cpp

Go to the documentation of this file.

//Global includes



//Pkg includes
#include <ros/ros.h>


//ISM includes
#include <ISM/tools/RecordedObjectsTransformer.hpp>


bool getNodeParameters(ros::NodeHandle nh, std::string& source_file, std::string& target_file, std::string& object_type, std::string& object_id,
                       std::vector<double>& position, std::vector<double>& orientation)
{
    bool success = true;

    if (!nh.getParam("source", source_file) || source_file.empty())
    {
        ROS_INFO("Missing parameter: \"source\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("source: " << source_file);
    }

    if (!nh.getParam("target", target_file) || target_file.empty())
    {
        ROS_INFO("Missing parameter: \"target\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("target: " << target_file);
    }

    if (!nh.getParam("object_type", object_type) || object_type.empty())
    {
        ROS_INFO("Missing parameter: \"object_type\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("object_type: " << object_type);
    }

    if (!nh.getParam("object_id", object_id) || object_id.empty())
    {
        ROS_INFO("Missing parameter: \"object_id\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("object_id: " << object_id);
    }

    if (!nh.getParam("position", position) || position.size() != 3)
    {
        ROS_INFO("Missing or not full defined parameter: \"position\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("position: " << position[0] << " " << position[1] << " " << position[2]);
    }

    if (!nh.getParam("orientation", orientation) || orientation.size() != 4)
    {
        ROS_INFO("Missing or not full defined parameter: \"orientation\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("orientation: " << orientation[0] << " " << orientation[1] << " " << orientation[2] << " " << orientation[3]);
    }

    return success;
}

int main (int argc, char **argv)
{
    //Usual ros node stuff
    ros::init(argc, argv, "recorded_objects_transformer");
    ros::NodeHandle nh("~");
    ISM::RecordedObjectsTransformer recorded_objects_transformer;

    std::string source_file;    
    std::string target_file;
    std::string object_type;
    std::string object_id;
    std::vector<double> point;
    std::vector<double> quat;


    if(getNodeParameters(nh, source_file, target_file, object_type, object_id, point, quat))
    {
        recorded_objects_transformer.transformRecordedObjects(source_file, target_file, object_type, object_id,
                                                              point[0], point[1], point[2], quat[0], quat[1], quat[2], quat[3]);
        ROS_INFO("TRANSFORMATION COMPLETE!");
    }
    else
    {
        ROS_INFO("TRANSFORMATION ABORTED! Check launch-file for missing parameter.");
    }

    return 0;
}