unlatching.h

Go to the documentation of this file.

#ifndef GRIPPING_H
#define GRIPPING_H
#include "ros/ros.h"
#include <geometry_msgs/PoseStamped.h> 
#include <actionlib/client/simple_action_client.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
// gripper control
#include <message_filters/subscriber.h>
#include <pr2_controllers_msgs/Pr2GripperCommandAction.h>

#include <ee_cart_imped_msgs/EECartImpedGoal.h>
#include <geometry_msgs/Twist.h>
// arm movement
#include <tf/transform_listener.h>
#include <std_srvs/Empty.h>

class EECartImpedArm; //forward declaration
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::Pr2GripperCommandAction> gripActClient;
//The policy merges kinect messages with approximately equal timestamp into one callback 
typedef message_filters::sync_policies::ApproximateTime<geometry_msgs::PoseStamped,
                                                        geometry_msgs::PoseStamped,
                                                        geometry_msgs::PoseStamped> MySyncPolicy;

class DoorOpener{
  public:
    DoorOpener(unsigned char side = 'r'); //< 'l' or 'r'
    ~DoorOpener();
    void grasp_handle(const boost::shared_ptr<const geometry_msgs::PoseStamped_<std::allocator<void> > > a_pregrasp,
                      const boost::shared_ptr<const geometry_msgs::PoseStamped_<std::allocator<void> > > a_grasp,
                      const boost::shared_ptr<const geometry_msgs::PoseStamped_<std::allocator<void> > > a_postgrasp) ;

    bool swing_door(geometry_msgs::PoseStamped);
    bool listen(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
    void addTrajectoryPoint(ee_cart_imped_msgs::EECartImpedGoal& msg, tf::Transform pose, float fx, float fy, float fz, float tx, float ty, float tz, float time, std::string frame_id);

    void drive_base(const ros::TimerEvent& event);
    void home_position();
    void use_gripper(float position, float effort);
    bool wait_for_gripper();
    
  private:
    gripActClient* gripperClient;
    ros::NodeHandle n;
    ros::ServiceServer serviceServer;
    message_filters::Subscriber<geometry_msgs::PoseStamped> pregrasp_sub;
    message_filters::Subscriber<geometry_msgs::PoseStamped> grasp_sub;
    message_filters::Subscriber<geometry_msgs::PoseStamped> postgrasp_sub; 
    message_filters::Synchronizer<MySyncPolicy> sync;
    ros::Publisher cmd_vel_pub_;
    geometry_msgs::Twist base_cmd;
    unsigned char side_;
    EECartImpedArm *arm_;
    tf::TransformListener* tflistener_; 
    std::string tool_frame_name_;
    std::string fixed_frame_name_; //fixed on robot, not map FIXME
    void push_door();
    void withdraw(float meters);
    tf::StampedTransform getTransform(const std::string& from, const std::string& to);
    bool listening_;
    bool started_;
    bool success_;

};
void printTransform(const tf::Transform& t, const std::string& name);
#endif