pose_interpolator.cpp

Go to the documentation of this file.

//Global includes



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


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


bool getNodeParameters(ros::NodeHandle nh, std::string& source_file, std::string& target_file, int& step_number)
{
    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("step_number", step_number))
    {
        ROS_INFO("Missing parameter: \"step_number\"");
        success = false;
    }
    else
    {
        ROS_INFO_STREAM("step_number: " << step_number);
    }

    return success;
}

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

    std::string target_file;
    std::string source_file;
    int step_number;

    if(getNodeParameters(nh, source_file, target_file, step_number))
    {
        if(step_number > 0)
        {
            pose_interpolator.interpolate(source_file, target_file, step_number);
            ROS_INFO("INTERPOLATION COMPLETED!");
        }
        else
        {
            ROS_INFO("CHECK step_number! It should have a value of 1 or greater.");
        }
    }
    else
    {
        ROS_INFO("INTERPOLATION ABORTED! Check launch-file for missing parameter.");
    }

    return 0;
}