ObjectMessageGenerator.h

Go to the documentation of this file.

#ifndef MOVEIT_OBJECTS_OBJECTMESSAGEGENERATOR_H
#define MOVEIT_OBJECTS_OBJECTMESSAGEGENERATOR_H

#include <ros/ros.h>

#include <object_msgs/Object.h>
#include <object_msgs/ObjectInfo.h>
#include <object_msgs/ObjectInfoRequest.h>
#include <object_msgs/ObjectInfoResponse.h>

#include <geometry_msgs/Pose.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_msgs/GetPlanningScene.h>
#include <moveit_msgs/PlanningScene.h>
#include <boost/thread/mutex.hpp>

#include <moveit_object_handling/MoveItCollisionMatrixManipulator.h>

namespace moveit_object_handling
{

class ObjectMessageGenerator
{
private:

    typedef object_msgs::Object ObjectMsg;
    typedef object_msgs::ObjectInfo ObjectInfoMsg;
    typedef std::map<std::string, moveit_msgs::CollisionObject> ObjToPublishMap;

public:

    ObjectMessageGenerator(ros::NodeHandle& _node_priv, ros::NodeHandle& _node);
    virtual ~ObjectMessageGenerator();

    bool isConnected() const;

private:


    void connectPub(const ros::SingleSubscriberPublisher& p);

    void disconnectPub(const ros::SingleSubscriberPublisher& p);

    void publishCollisionsEvent(const ros::TimerEvent& e);

    void receiveObject(const ObjectMsg& msg);

    moveit_msgs::CollisionObject getCollisionGeometry(const std::string& name);

    void updatePose(const ObjectMsg& newObj, moveit_msgs::CollisionObject& obj);

    moveit_msgs::CollisionObject transferContent(const ObjectMsg& msg, bool skipGeometry);

    std::vector<moveit_msgs::CollisionObject> getCurrentCollisionObjects(bool only_names = true);
    std::set<std::string> getCurrentCollisionObjectNames();

    std::vector<moveit_msgs::AttachedCollisionObject> getCurrentAttachedCollisionObjects();
    std::set<std::string> getCurrentAttachedCollisionObjectNames();

    ros::NodeHandle node_priv, node;
    std::string OBJECTS_TOPIC;
    std::string REQUEST_OBJECTS_TOPIC;
    std::string COLLISION_OBJECT_TOPIC;
    std::string GET_PLANNING_SCENE_SERVICE;
    std::string SET_PLANNING_SCENE_TOPIC;
    float PUBLISH_COLLISION_RATE;
    bool USE_PLANNING_SCENE_DIFF;

    ros::Publisher collision_pub;
    ros::Publisher planning_scene_pub;

    ros::Subscriber object_sub;

    ros::ServiceClient object_info_client;

    ros::ServiceClient planning_scene_client;


    // all objects which were already added
    std::set<std::string> addedObjects;

    // objects to skip as collision objects:
    // whenever an Object.msg message arrives to
    // add this object name, skip it.
    std::set<std::string> skipObjects;

    // links of the robot to always be allowed to
    // collide with *any* new collision object arriving.
    // Technically can be used for any objects (not only
    // robot links), but it's intended to disable collisions
    // with parts of the robot only.
    std::vector<std::string> allowedCollisionLinks;

    MoveItCollisionMatrixManipulator acmManip;

    ObjToPublishMap objsToPublish;

    boost::mutex mutex; //mutex for addedObjects and objsToPublish

    ros::Timer publishCollisionsTimer;

    //is set to false until addedObjects is initialized
    bool initExistingObj;
};

}  // namespace moveit_object_handling

#endif  // MOVEIT_OBJECTS_OBJECTMESSAGEGENERATOR_H