util.h

Go to the documentation of this file.

/*
 * utils.h
 *
 *  Created on: Aug 6, 2010
 *      Author: jscholz
 */

#ifndef UTILS_H_
#define UTILS_H_

#include <ros/ros.h>
#include <geometry_msgs/PolygonStamped.h>
#include <LinearMath/btTransform.h>
#include <string>

namespace cart_state_estimator {

using std::string;

template<typename T>
T getPrivateParam(const std::string& name, const T& default_val) {
        T val;
        ros::NodeHandle nh("~");
        nh.param(name, val, default_val);
        ROS_DEBUG_STREAM_NAMED ("init", "Set param " << name << " to value "
                        << val << " (default was " << default_val << ")");
        return val;
}

template<typename T>
T getParam(const std::string& param, const std::string& ns = "~") {
        ros::NodeHandle nh(ns);
        ROS_DEBUG_STREAM("LOADING "<< param << " from " << nh.getNamespace());
        T val;
        bool found = nh.getParam(param, val);
        ROS_ASSERT_MSG(found, "Did not find param %s in %s", param.c_str(), nh.getNamespace().c_str());
        return val;
}

btTransform readTransformParameter(const std::string& ns) {
        using XmlRpc::XmlRpcValue;
        XmlRpcValue position = getParam<XmlRpcValue> ("position", ns);
        XmlRpcValue orientation = getParam<XmlRpcValue> ("orientation", ns);
        ROS_ASSERT(position.getType() == XmlRpc::XmlRpcValue::TypeArray &&
                        orientation.getType() == XmlRpc::XmlRpcValue::TypeArray &&
                        position.size() == 3 && orientation.size() == 4);
        const double x = position[0];
        const double y = position[1];
        const double z = position[2];
        const btVector3 tr(x, y, z);
        const btQuaternion q(orientation[0], orientation[1], orientation[2],
                        orientation[3]);

        { // debugging
                btTransform trans(q, tr);
                geometry_msgs::Transform transform;
                tf::transformTFToMsg(trans, transform);
                ROS_DEBUG_STREAM ("read param from " << ns << " into " << transform);
        }
        return btTransform(q, tr);
}
} // namespace cart_state_estimator
#endif /* UTILS_H_ */