00001
00002
00003
00004
00005
00006
00007
00008 #ifndef UTILS_H_
00009 #define UTILS_H_
00010
00011 #include <ros/ros.h>
00012 #include <std_msgs/ColorRGBA.h>
00013 #include <boost/format.hpp>
00014 #include <tf/transform_broadcaster.h>
00015 #include <tf/transform_listener.h>
00016 #include "articulation_models/utils.h"
00017
00018 #ifndef SQR
00019 #define SQR(a) ((a)*(a))
00020 #endif
00021
00022 std_msgs::ColorRGBA HSV_to_RGB(double h,double s,double v );
00023 btMatrix3x3 RPY_to_MAT(double roll, double pitch, double yaw);
00024 void MAT_to_RPY(const btMatrix3x3& mat, double &roll, double &pitch, double &yaw);
00025
00026 #define VEC(p1) "["<< \
00027 p1.getOrigin().x() << "," << p1.getOrigin().y()<<","<<p1.getOrigin().z()<< \
00028 "/"<< \
00029 p1.getRotation().x() << "," << p1.getRotation().y()<<","<<p1.getRotation().z()<<","<<p1.getRotation().w()<< \
00030 "]"
00031
00032 std::string transform_to_string(const btTransform &p);
00033
00034 std::string pose_to_string(const geometry_msgs::Pose &pose);
00035
00036 std::string uncert_to_string(double sigma_position,double sigma_orientation);
00037
00038 double getBIC(double loglh, size_t k, size_t n);
00039
00040 #endif
