stateCreatorRobotPose.h

Go to the documentation of this file.

#ifndef STATE_CREATOR_ROBOT_POSE_H
#define STATE_CREATOR_ROBOT_POSE_H

#include "continual_planning_executive/stateCreator.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <ros/ros.h>
#include "tidyup_actions/robotPoseVisualization.h"

namespace tidyup_actions 
{


    class StateCreatorRobotPose : public continual_planning_executive::StateCreator
    {
        public:
            StateCreatorRobotPose();
            ~StateCreatorRobotPose();


            virtual void initialize(const std::deque<std::string> & arguments);

            virtual bool fillState(SymbolicState & state);

        protected:

            bool extractPoseStamped(const SymbolicState & state, const string & object,
                    geometry_msgs::PoseStamped & pose) const;

            std_msgs::ColorRGBA getLocationColor(const SymbolicState & state, const string & location) const;

            visualization_msgs::MarkerArray getLocationMarkers(const SymbolicState & state, const string & location,
                    const string & ns, int id, bool useMeshes) const;

            void publishLocationsAsMarkers(const SymbolicState & state);

            sensor_msgs::JointState getArmSideJointState() const;

        protected:
            tf::TransformListener _tf;

            double _goalToleranceXY;
            double _goalToleranceYaw;

            static const bool s_PublishLocationsAsMarkers = true;
            static const bool s_PublishMeshMarkers = true;

            ros::Publisher _markerPub;

            std::string _robotPoseObject;   
            std::string _robotPoseType;     
            std::string _atPredicate;       
            std::string _locationType;      

            mutable RobotPoseVisualization _robotPoseVis;
    };

};

#endif