Go to the documentation of this file.00001
00002 #include <tf/tf.h>
00003 #include <std_msgs/String.h>
00004 #include <sensor_msgs/LaserScan.h>
00005 #include <sensor_msgs/Image.h>
00006 #include <geometry_msgs/PoseStamped.h>
00007 #include <nav_msgs/OccupancyGrid.h>
00008 #include <map_messages/PointOfInterest.h>
00009 #include <map_messages/DeletePointOfInterest.h>
00010 #include <map_messages/PointsOfInterest.h>
00011
00012 #include "QtRosNode.h"
00013 #include "Workers/PointOfInterest/PointOfInterest.h"
00014
00015 QtRosNode::QtRosNode(int argc, char *argv[], const char* node_name, MainWindow* mainWindow)
00016 {
00017 ros::init(argc, argv, node_name);
00018 ros::start();
00019 m_NodeHandle = new ros::NodeHandle;
00020 m_MainWindow = mainWindow;
00021
00022
00023 quitfromgui = false;
00024
00025
00026 advertiseTopics();
00027 subscribeToTopics();
00028
00029 start();
00030
00031
00032 ADD_MACHINE_STATE( m_ModuleMachine, IDLE );
00033 ADD_MACHINE_STATE( m_ModuleMachine, WAITING );
00034 ADD_MACHINE_STATE( m_ModuleMachine, DOOR_IS_OPEN );
00035
00036
00037 m_ModuleMachine.setName( "Module State" );
00038 m_ModuleMachine.setState( IDLE );
00039
00040 }
00041
00042 QtRosNode::~QtRosNode()
00043 {
00044 if( m_NodeHandle ) delete m_NodeHandle;
00045 if(ros::isStarted()) {
00046 ros::shutdown();
00047 ros::waitForShutdown();
00048 }
00049 wait();
00050 }
00051
00052
00053 void QtRosNode::compareRanges()
00054 {
00055
00056
00057
00058
00059
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084 }
00085
00086
00087 void QtRosNode::subscribeToTopics()
00088 {
00089
00090 m_LaserSubscriber = m_NodeHandle->subscribe<sensor_msgs::LaserScan>("/scan", 10, &QtRosNode::callbackLaserScan, this);
00091
00092 m_PoseStampedSubscriber = m_NodeHandle->subscribe<geometry_msgs::PoseStamped>("/pose", 10, &QtRosNode::callbackPoseStamped, this);
00093 m_MapDataSubscriber = m_NodeHandle->subscribe<nav_msgs::OccupancyGrid>("/map", 10, &QtRosNode::callbackOccupancyGrid, this);
00094 m_POIsSubscriber = m_NodeHandle->subscribe<map_messages::PointsOfInterest>("/map_manager/poi_list", 10, &QtRosNode::callbackPOIList, this);
00095
00096
00097
00098
00099 m_ORLearningStatusSubscriber = m_NodeHandle->subscribe<or_msgs::OrLearningStatus>("/or/learning_status", 10, &QtRosNode::callbackLearningStatus, this);
00100 m_ORObjectNamesSubscriber = m_NodeHandle->subscribe<or_msgs::OrObjectNames>("or/obj_names", 10, &QtRosNode::callbackObjectNames, this);
00101 }
00102
00103 void QtRosNode::advertiseTopics()
00104 {
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116 }
00117
00118
00119 void QtRosNode::quitNow()
00120 {
00121 quitfromgui = true;
00122 }
00123
00124
00125 void QtRosNode::run()
00126 {
00127 ros::Rate loop_rate(10);
00128
00129 while (ros::ok())
00130 {
00131 ros::spinOnce();
00132 loop_rate.sleep();
00133 }
00134 emit rosShutdown();
00135
00136 }
00137
00138 void QtRosNode::callbackPoseStamped(const geometry_msgs::PoseStamped::ConstPtr &msg)
00139 {
00140 double theta = tf::getYaw(msg->pose.orientation);
00141 emit poseUpdated(msg->pose.position.x, msg->pose.position.y, theta);
00142 }
00143
00144 void QtRosNode::callbackOccupancyGrid(const nav_msgs::OccupancyGrid::ConstPtr &msg)
00145 {
00146
00147 unsigned char* map = new unsigned char[msg->data.size()];
00148
00149
00150 int minX = INT_MIN;
00151 int minY = INT_MIN;
00152 int maxX = INT_MAX;
00153 int maxY = INT_MAX;
00154 for(int x = 0; x < msg->info.height; x++)
00155 {
00156 int xOffset = msg->info.height * x;
00157 for(int y = 0; y < msg->info.width; y++)
00158 {
00159 int i = xOffset + y;
00160 map[i] = msg->data[i] == -1 ? 50 : msg->data[i];
00161 if(map[i]!=-1) {
00162 if(minX==INT_MIN || minX > x)
00163 minX = x;
00164 if(minY==INT_MIN || minY > y)
00165 minY = y;
00166 if(maxX==INT_MAX || maxX < x)
00167 maxX = x;
00168 if(maxY==INT_MAX || maxY < y)
00169 maxY = y;
00170 }
00171 }
00172 }
00173
00174 emit mapUpdated(map, msg->info.width, msg->info.resolution, msg->info.origin, minX, minY, maxX, maxY);
00175 }
00176
00177 void QtRosNode::callbackPOIList(const map_messages::PointsOfInterest::ConstPtr& msg)
00178 {
00179 emit poiListUpdated(msg->pois);
00180
00181 }
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195 void QtRosNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
00196 {
00197
00198
00199
00200
00201
00202
00203
00204
00205
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246 }
00247
00248 void QtRosNode::callbackLearningStatus(const or_msgs::OrLearningStatusConstPtr &msg)
00249 {
00250 emit learningStatus(msg->image_names, msg->object_type);
00251 }
00252
00253 void QtRosNode::callbackObjectNames(const or_msgs::OrObjectNamesConstPtr &msg)
00254 {
00255 emit objectNames(msg->object_names, msg->object_types);
00256 }