ROSFunctions.h

Go to the documentation of this file.

#ifndef CONVENIENCE_ROS_FUNCTIONS_ROSFUNCTIONS_H
#define CONVENIENCE_ROS_FUNCTIONS_ROSFUNCTIONS_H

#include <baselib_binding/SharedPtr.h>
#include <baselib_binding/Thread.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>
#include <actionlib/client/simple_client_goal_state.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <sensor_msgs/JointState.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <fstream>

#include <boost/foreach.hpp>
// XXX TODO: get rid of direct boost dependency

#define DEFAULT_TF_CACHE_TIME 15

namespace convenience_ros_functions
{

class ROSFunctions
{
public:
    typedef baselib_binding::shared_ptr<ROSFunctions>::type ROSFunctionsPtr;
    ROSFunctions(float tf_max_cache_time = DEFAULT_TF_CACHE_TIME);

    ~ROSFunctions()
    {
    }

    static void initSingleton();
    static void destroySingleton();

    static ROSFunctionsPtr Singleton();

    template<typename ROSMessage>
    static bool saveToFile(const ROSMessage& msg, const std::string& filename, bool asBinary);

    template<typename ROSMessage>
    static bool readFromFile(const std::string& filename, ROSMessage& msg, bool isBinary);

    template<typename ROSMessage>
    static bool saveToBagFile(const std::vector<ROSMessage>& msgs, const std::string& filename);

    template<typename ROSMessage>
    static bool readFromBagFile(const std::string& filename, std::vector<ROSMessage>& msgs);

    static void assignJointState(const sensor_msgs::JointState& target_joints,
                                 sensor_msgs::JointState& joint_state);


    static bool intersectJointState(const sensor_msgs::JointState& s1, const sensor_msgs::JointState& s2,
                                    sensor_msgs::JointState& result,
                                    bool init_s1, bool s2_is_subset);

    static bool intersectJointStates(const sensor_msgs::JointState& s1,
                                     const sensor_msgs::JointState& s2,
                                    sensor_msgs::JointState& result, bool s2_is_subset);


    static bool equalJointPositionsSimple(const sensor_msgs::JointState& j1, const sensor_msgs::JointState& j2,
                                const float pos_tolerance);
   
    static int equalJointPositions(const sensor_msgs::JointState& j1, const sensor_msgs::JointState& j2,
                                const float pos_tolerance);


    static bool getJointStateAt(int idx, const trajectory_msgs::JointTrajectory& traj, sensor_msgs::JointState& result);


    template<typename MJointState>
    static bool getPoseFromVirtualJointState(const std::vector<std::string>& joint_names,
            const MJointState& virtualJointState,
            const std::string& virtualJointName, geometry_msgs::PoseStamped& robotPose);

    bool canGetTransform(const std_msgs::Header& p1, const std_msgs::Header& p2, bool latest, bool printError) const;
    bool canGetTransform(const std::string& f1, const std::string& f2, const ros::Time& useTime, bool printError) const;

    bool waitForTransform(const std_msgs::Header& p1, const std_msgs::Header& p2,
                          const float& timeout, bool latest, bool printError);
    bool waitForTransform(const std::string& f1, const std::string& f2,
                          const ros::Time& useTime, const float& timeout, bool printError);

    int equalPoses(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2, float accuracy_pos,
                   float accuracy_ori, bool useLatestTime, float maxWaitTransform, bool printErrors = true);

    int poseDistance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2, float& posDist, float& angleDist,
                     bool useLatestTime, float maxWaitTransform, bool printErrors);

    int relativePose(const geometry_msgs::PoseStamped& origin, const geometry_msgs::PoseStamped& other,
                     geometry_msgs::Pose& result, bool useLatestTime, float maxWaitTransform, bool printErrors = true);

    int transformPose(const geometry_msgs::PoseStamped& p, const std::string& frame_id, geometry_msgs::PoseStamped& result,
                      float maxWaitTransform, bool printErrors = true);

    int getTransform(const std::string& f1, const std::string& f2, geometry_msgs::Pose& result,
                     const ros::Time& useTime, float maxWaitTransform, bool printErrors = true);

    static void applyTransform(const geometry_msgs::Pose& transform, geometry_msgs::Pose& pose);

    template<typename GH, typename Res>
    static void mapToGoalHandle(const actionlib::SimpleClientGoalState& state, GH& goalHandle,
                                const Res& res, const std::string& s);

    template<typename GH, typename Res>
    static void effectOnGoalHandle(const actionlib::SimpleClientGoalState& state, GH& goalHandle,
                                   const Res& res, const std::string& s = "");
    template<typename GH>
    static void effectOnGoalHandle(const actionlib::SimpleClientGoalState& state, GH& goalHandle);

private:
    static int hasVal(const std::string& val, const std::vector<std::string>& vec);

    //static lock for general method access
    static baselib_binding::recursive_mutex slock;
    static ROSFunctionsPtr _singleton;

    //tf_listener is thread-safe so doesn't require a mutex.
    tf::TransformListener tf_listener;
};

#include <convenience_ros_functions/ROSFunctions.hpp>

}

#endif // CONVENIENCE_ROS_FUNCTIONS_ROSFUNCTIONS_H