00001 #ifndef ___ROBOTTHREAD_H___ 00002 #define ___ROBOTTHREAD_H___ 00003 00004 #include <QThread> 00005 #include <QObject> 00006 #include <QStringList> 00007 #include <stdlib.h> 00008 #include <iostream> 00009 #include "assert.h" 00010 00011 #include <ros/ros.h> 00012 #include <ros/network.h> 00013 #include <std_msgs/String.h> 00014 #include <sensor_msgs/LaserScan.h> 00015 #include <geometry_msgs/Twist.h> 00016 #include <nav_msgs/Odometry.h> 00017 #include <actionlib/client/simple_action_client.h> 00018 #include <move_base_msgs/MoveBaseAction.h> 00019 00020 typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient; 00021 00022 namespace server { 00023 class RobotThread : public QThread { 00024 Q_OBJECT 00025 public: 00026 RobotThread(int argc, char **pArgv); 00027 virtual ~RobotThread(); 00028 00029 double getXPos(); 00030 double getXSpeed(); 00031 double getASpeed(); 00032 double getYPos(); 00033 double getAPos(); 00034 00035 bool init(); 00036 00037 void callback(nav_msgs::Odometry msg); 00038 void scanCallBack(sensor_msgs::LaserScan scan); 00039 00040 void SetSpeed(double speed, double angle); 00041 void setPose(QList<double> to_set); 00042 void goToXYZ(geometry_msgs::Point goTo); 00043 void setCommand(QString cmd); 00044 void run(); 00045 00046 private: 00047 QString command; 00048 00049 int m_Init_argc; 00050 char** m_pInit_argv; 00051 00052 double m_speed; 00053 double m_angle; 00054 00055 double m_xPos; 00056 double m_yPos; 00057 double m_aPos; 00058 00059 double m_maxRange; 00060 double m_minRange; 00061 00062 QList<double> ranges; 00063 00064 ros::Publisher cmd_publisher; 00065 ros::Publisher sim_velocity; 00066 00067 ros::Subscriber pose_listener; 00068 ros::Subscriber scan_listener; 00069 }; 00070 }//end namespace 00071 #endif