Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031 #ifndef __ROBOT_DRIVER_H__
00032 #define __ROBOT_DRIVER_H__
00033
00034 #include <ros/ros.h>
00035
00036 #include <actionlib/client/simple_action_client.h>
00037 #include <actionlib/client/terminal_state.h>
00038 #include <pr2_common_action_msgs/ArmMoveIKAction.h>
00039 #include <tf/transform_listener.h>
00040 #include <move_base_msgs/MoveBaseAction.h>
00041 #include <geometry_msgs/Twist.h>
00042 #include <sensor_msgs/LaserScan.h>
00043 #include <sensor_msgs/PointCloud.h>
00044 #include <laser_geometry/laser_geometry.h>
00045 #include <boost/thread/mutex.hpp>
00046
00047
00048 class RobotDriver
00049 {
00050 private:
00052 ros::NodeHandle nh_;
00054 ros::Publisher cmd_vel_pub_;
00056 tf::TransformListener listener_;
00058 ros::Subscriber subScan_;
00059 laser_geometry::LaserProjection *projector_;
00060 boost::mutex scan_mutex;
00061 bool weHaveScan;
00062 double scanPoints[5000][2];
00063 size_t numScanPoints;
00064
00066 RobotDriver();
00067
00068 static RobotDriver *instance;
00069
00070 void QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler);
00071
00072 public:
00073
00074 static RobotDriver *getInstance();
00075
00076 void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in);
00077
00079 bool checkCollision(double relativePose[]);
00080
00081 bool checkCollision(tf::Stamped<tf::Pose> target);
00082
00083 void getRobotPose(tf::Stamped<tf::Pose> &marker);
00084
00085
00086 bool driveInMap(tf::Stamped<tf::Pose> targetPose,bool exitWhenStuck = false);
00087
00089 bool driveInMap(const double targetPose[],bool exitWhenStuck = false);
00090
00091 bool driveInOdom(const double targetPose[], bool exitWhenStuck = false);
00092
00093 void moveBase(const double pose[], bool useNavigation = false);
00094
00095 static void moveBase4(double x, double y, double oz, double ow, bool useNavigation = false);
00096
00097 void moveBaseP(double x, double y, double oz, double ow, bool useNavigation = false);
00098
00099 void driveToMatch(std::vector<tf::Stamped<tf::Pose> > targetPose, std::vector<std::string> frame_ids);
00100
00101 };
00102
00103 #endif