found_object_handler.hpp

Go to the documentation of this file.

#pragma once

// Global Inclusion
#include <boost/shared_ptr.hpp>
#include <vector>
#include <map>
#include <list>
#include <eigen3/Eigen/Geometry>


// ROS Main Inclusion
#include <ros/ros.h>


// ROS-wide Inclusion
#include <asr_msgs/AsrObject.h>
#include <asr_object_database/ObjectMetaData.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_listener.h>


// Local Inclusion
#include "asr_world_model/GetFoundObjectList.h"
#include "asr_world_model/PushFoundObject.h"
#include "asr_world_model/PushFoundObjectList.h"
#include "asr_world_model/VisualizeSampledPoses.h"

#include "world_model/model/settings.hpp"
#include "world_model/model/model_type.hpp"
#include "world_model/model/model_object.hpp"
#include "world_model/helper/debug_helper.hpp"
#include "world_model/helper/pose_helper.hpp"
#include "world_model/world_model_visualizer_rviz.hpp"

namespace world_model
{


class FoundObjectHandler
{
public:
    /* ----------------- Public members  ------------------  */
    // Wrapped Constants

    // PushFoundObject
    static const inline std::string GetPushFoundObjectServiceName()
    { return "push_found_object"; }

    // PushFoundObjectList
    static const inline std::string GetPushFoundObjectListServiceName()
    { return "push_found_object_list"; }

    // EmptyFoundObjectList
    static const inline std::string GetEmptyFoundObjectListServiceName()
    { return "empty_found_object_list"; }

    // GetFoundObjectList
    static const inline std::string GetGetFoundObjectListServiceName()
    { return "get_found_object_list"; }

    // VisualizeSampledPoses
    static const inline std::string GetVisualizeSampledPosesName()
    { return "visualize_sampled_poses"; }

private:
    const double DEG_TO_RAD = M_PI / 180.0;
    const double EPSILON = 0.0001;

    /* ----------------- Private members  ------------------  */
    DebugHelperPtr debug_helper_ptr_;
    PoseHelperPtr pose_helper_ptr_;
    tf::TransformListener tf_transform_listener_;

    // Services
    WorldModelVisualizerRVIZPtr world_model_visualizer_ptr_;

    // Clients
    boost::shared_ptr<ros::ServiceClient> object_type_client_ptr_;

    ModelTypePtrPerTypeMapPtr model_type_ptr_per_type_map_ptr_;

    SettingsPtr settings_ptr_;

public:
    /* ----------------- Public functions  ------------------  */
    FoundObjectHandler(boost::shared_ptr<ros::Publisher> marker_publisher_ptr,
                       boost::shared_ptr<ros::ServiceClient> object_type_client_ptr,
                       ModelTypePtrPerTypeMapPtr model_type_ptr_per_type_map_ptr,
                       SettingsPtr settings_ptr);

    bool processEmptyFoundObjectListServiceCall(std_srvs::Empty::Request &request,
                                                std_srvs::Empty::Response &response);

    bool processGetFoundObjectListServiceCall(asr_world_model::GetFoundObjectList::Request &request,
                                              asr_world_model::GetFoundObjectList::Response &response);
    bool processPushFoundObjectListServiceCall(asr_world_model::PushFoundObjectList::Request &request,
                                               asr_world_model::PushFoundObjectList::Response &response);

    inline bool processPushFoundObjectServiceCall(asr_world_model::PushFoundObject::Request &request,
                                                  asr_world_model::PushFoundObject::Response &response)
    {
        debug_helper_ptr_->write(std::stringstream() << "Calling processPushFoundObjectServiceCall", (DebugHelper::SERVICE_CALLS + DebugHelper::FOUND_OBJECT));
        return pushFoundObject(request.found_object);
    }

    bool processVisualizeSampledPosesCall(asr_world_model::VisualizeSampledPoses::Request &request,
                                          asr_world_model::VisualizeSampledPoses::Response &response);

private:
    /* ----------------- Private functions  ------------------  */

    bool pushFoundObject(asr_msgs::AsrObject &candidate);

    void updateBestHypothesis(ModelObjectPtr &currentModelObjectPtr, const asr_msgs::AsrObject &candidate);

    bool sampleFoundObject(asr_msgs::AsrObject &candidate);

    bool transformToBaseFrame(asr_msgs::AsrObject &candidate) const;

    bool evaluateNeighborhood(const asr_msgs::AsrObject &object1, const asr_msgs::AsrObject &object2) const;


    //COMMENT
    void visualizeFoundObjects();

    std::vector<geometry_msgs::Pose> interpolateOrientationAroundAxis(
            const geometry_msgs::Pose &poseToInterpolate, const double interpolationOffset, const Eigen::Vector3d &axisToRotate, const int numberOfInterpolationsPerOrientation) const;

    geometry_msgs::Pose rotateOrientationAroundAxis(const geometry_msgs::Pose &poseToRotate, const double interpolationOffset, const Eigen::Vector3d &axisToRotate) const;

    std::vector<geometry_msgs::Pose> interpolatePoses(const geometry_msgs::Pose &fromPose, const geometry_msgs::Pose &toPose, const int numberOfInterpolations) const;

};


inline bool comparePbdObjectCluster (const PbdObjectCluster& first, const PbdObjectCluster& second)
{
    return ( first.second > second.second );
}

}