00001
00002
00003
00004
00005
00006
00007
00008 #ifndef HEAD_CONTROL_H_
00009 #define HEAD_CONTROL_H_
00010
00011
00012 #include <string>
00013
00014 #include <ros/ros.h>
00015 #include <pr2_controllers_msgs/PointHeadAction.h>
00016 #include <actionlib/client/simple_action_client.h>
00017 #include <geometry_msgs/PoseStamped.h>
00018 #include <tf/transform_listener.h>
00019
00020
00021 namespace simple_robot_control{
00022
00023 using std::string;
00024
00025 static const string DEFAULT_HEAD_POINTING_FRAME = "high_def_frame";
00026
00027 class Head{
00028 private:
00029 actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction>* traj_client_;
00030
00031 const static double DEFAULT_SPEED = 1.0;
00032
00033 public:
00034 Head();
00035 ~Head();
00036
00037
00038 bool lookat( const geometry_msgs::PoseStampedConstPtr pose, double speed = DEFAULT_SPEED, string pointing_frame = DEFAULT_HEAD_POINTING_FRAME);
00039 bool lookat( const tf::StampedTransform pose, double speed = DEFAULT_SPEED, string pointing_frame = DEFAULT_HEAD_POINTING_FRAME);
00040 bool lookat( const string source_frame, double x, double y, double z, double speed = DEFAULT_SPEED, string pointing_frame = DEFAULT_HEAD_POINTING_FRAME);
00041 bool lookat( const string source_frame, tf::Vector3 position = tf::Vector3() , double speed = DEFAULT_SPEED, string pointing_frame = DEFAULT_HEAD_POINTING_FRAME);
00042 };
00043
00044 }
00045
00046 #endif