asr_flir_ptu_driver: PTU_GUI.cpp Source File

Go to the documentation of this file.
00001 
00008 #include "gui/PTU_GUI.h"
00009 
00010 namespace GUI_PTU {
00011 
00012 PTU_GUI::PTU_GUI( wxWindow* parent ) : GUIDialog( parent )
00013 {
00014         update_timer = new wxTimer(this);
00015         update_timer->Start(16);
00016 
00017         immUpdate = false;
00018         listenForUpdates = false;
00019 
00020         Connect(update_timer->GetId(), wxEVT_TIMER, wxTimerEventHandler(PTU_GUI::onUpdate), NULL, this);
00021 
00022         initTopicList();
00023 
00024 //      ros::NodeHandle priv("~");
00025 
00026         panSlider->SetValue(0);
00027         panSpinner->SetValue(0);
00028         tiltSlider->SetValue(0);
00029         tiltSpinner->SetValue(0);
00030 
00031         pan_min->SetValue(0);
00032         pan_max->SetValue(0);
00033         pan_base->SetValue(0);
00034         pan_target->SetValue(0);
00035         pan_upper->SetValue(0);
00036         pan_accel->SetValue(0);
00037         pan_hold->SetValue(0);
00038         pan_move->SetValue(0);
00039 
00040         tilt_min->SetValue(0);
00041         tilt_max->SetValue(0);
00042         tilt_base->SetValue(0);
00043         tilt_target->SetValue(0);
00044         tilt_upper->SetValue(0);
00045         tilt_accel->SetValue(0);
00046         tilt_hold->SetValue(0);
00047         tilt_move->SetValue(0);
00048 
00049         leftImageSubscriber = nh.subscribe<sensor_msgs::Image>("left/image_color", 1, &PTU_GUI::onLeftImage, this);
00050         rightImageSubscriber = nh.subscribe<sensor_msgs::Image>("right/image_color", 1, &PTU_GUI::onRightImage, this);
00051 
00052     predict_client = nh.serviceClient<asr_flir_ptu_driver::Predict>("/asr_flir_ptu_driver/path_prediction");
00053 
00054         wxBitmap* imageLeft = new wxBitmap(wxImage(640, 480));
00055         leftPanel->setImage(imageLeft);
00056         leftPanel->paintNow();
00057 
00058         wxBitmap* imageRight = new wxBitmap(wxImage(640, 480));
00059         rightPanel->setImage(imageRight);
00060         rightPanel->paintNow();
00061     seq_num = 0;
00062     ptu.getParam("asr_flir_ptu_gui/path_prediction", use_path_prediction);
00063 }
00064 
00065 void PTU_GUI::updatePTUInfo() {
00066     jointStatePublisher = ptu.advertise<asr_flir_ptu_driver::State>("asr_flir_ptu_driver/state_cmd", 1);
00067     jointStateSubscriber = ptu.subscribe<asr_flir_ptu_driver::State>("asr_flir_ptu_driver/ptu_state", 1, &PTU_GUI::onStateCommand, this);
00068     updater = ptu.serviceClient<std_srvs::Empty>("asr_flir_ptu_driver/update_settings");
00069 
00070     updateSlidersFromParamServer();
00071 
00072         panSlider->SetValue(0);
00073         panSpinner->SetValue(0);
00074         tiltSlider->SetValue(0);
00075         tiltSpinner->SetValue(0);
00076 }
00077 
00078 void PTU_GUI::updateSlidersFromParamServer()
00079 {
00080     int panBase = 0, panSpeed = 0, panUpper = 0, panAccel = 0;
00081     int tiltBase = 0, tiltSpeed = 0, tiltUpper = 0, tiltAccel = 0;
00082     int panHold = 0, panMove = 0;
00083     int tiltHold = 0, tiltMove = 0;
00084     double panMin = 0, panMax = 0;
00085     double tiltMin = 0, tiltMax = 0;
00086 
00087     ptu.getParam("asr_flir_ptu_driver/pan_min_angle", panMin);
00088     ptu.getParam("asr_flir_ptu_driver/pan_max_angle", panMax);
00089     ptu.getParam("asr_flir_ptu_driver/pan_base_speed", panBase);
00090     ptu.getParam("asr_flir_ptu_driver/pan_target_speed", panSpeed);
00091     ptu.getParam("asr_flir_ptu_driver/pan_upper_speed", panUpper);
00092     ptu.getParam("asr_flir_ptu_driver/pan_accel", panAccel);
00093     ptu.getParam("asr_flir_ptu_driver/pan_hold", panHold);
00094     ptu.getParam("asr_flir_ptu_driver/pan_move", panMove);
00095     ptu.getParam("asr_flir_ptu_driver/tilt_min_angle", tiltMin);
00096     ptu.getParam("asr_flir_ptu_driver/tilt_max_angle", tiltMax);
00097     ptu.getParam("asr_flir_ptu_driver/tilt_base_speed", tiltBase);
00098     ptu.getParam("asr_flir_ptu_driver/tilt_target_speed", tiltSpeed);
00099     ptu.getParam("asr_flir_ptu_driver/tilt_upper_speed", tiltUpper);
00100     ptu.getParam("asr_flir_ptu_driver/tilt_accel", tiltAccel);
00101     ptu.getParam("asr_flir_ptu_driver/tilt_hold", tiltHold);
00102     ptu.getParam("asr_flir_ptu_driver/tilt_move", tiltMove);
00103 
00104     pan_min->SetValue((int) panMin);
00105     pan_max->SetValue((int) panMax);
00106     pan_base->SetValue(panBase);
00107     pan_target->SetValue(panSpeed);
00108     pan_upper->SetValue(panUpper);
00109     pan_accel->SetValue(panAccel);
00110     pan_hold->SetValue(panHold);
00111     pan_move->SetValue(panMove);
00112 
00113     tilt_min->SetValue((int) tiltMin);
00114     tilt_max->SetValue((int) tiltMax);
00115     tilt_base->SetValue(tiltBase);
00116     tilt_target->SetValue(tiltSpeed);
00117     tilt_upper->SetValue(tiltUpper);
00118     tilt_accel->SetValue(tiltAccel);
00119     tilt_hold->SetValue(tiltHold);
00120     tilt_move->SetValue(tiltMove);
00121 }
00122 
00123 
00124 void PTU_GUI::initTopicList() {
00125          ros::master::V_TopicInfo topics;
00126          ros::master::getTopics(topics);
00127          ros::master::V_TopicInfo::iterator it = topics.begin();
00128          ros::master::V_TopicInfo::iterator end = topics.end();
00129 
00130          int topicCount = 0;
00131 
00132          for (; it != end; ++it) {
00133                  const ros::master::TopicInfo& ti = *it;
00134 
00135                  if (strcmp(ti.datatype.c_str(), "sensor_msgs/Image") == 0) {
00136                          leftImageTopic->Append(wxString::FromUTF8(ti.name.c_str()));
00137                          rightImageTopic->Append(wxString::FromUTF8(ti.name.c_str()));
00138                          topicCount++;
00139          } else if (strcmp(ti.datatype.c_str(), "asr_flir_ptu_driver/State") == 0 &&
00140                                  ti.name.find("state") != std::string::npos) {
00141                          wxString entryName = wxString::FromUTF8(ti.name.substr(0, ti.name.find("state")).c_str());
00142                          if (ptuChoice->FindString(entryName) == wxNOT_FOUND) ptuChoice->Append(entryName);
00143                  }
00144          }
00145 
00146          //these lines assume that the combo boxes are sorted, which they aren't
00147          for (long int i = 0; i < topicCount; i++) {
00148                  if (leftImageTopic->GetString(i).Find(wxString::FromUTF8(DEFAULT_PREFFEREDLEFT)) >= 0) {
00149                          leftImageTopic->SetSelection(i);
00150                          break;
00151                  }
00152          }
00153 
00154          for (long int i = 0; i < topicCount; i++) {
00155                  if (rightImageTopic->GetString(i).Find(wxString::FromUTF8(DEFAULT_PREFFEREDRIGHT)) >= 0) {
00156                          rightImageTopic->SetSelection(i);
00157                          break;
00158                  }
00159          }
00160 }
00161 
00162 void PTU_GUI::OnDialogClose( wxCloseEvent& event ) {
00163         //setting the checkbox seems to suppress the bug where the cam tries to turn around 180°
00164         immCheck->SetValue(false);
00165         wxTheApp->Exit();
00166 }
00167 
00168 void PTU_GUI::OnLeftTopicChoice( wxCommandEvent& event ) {
00169         leftImageSubscriber.shutdown();
00170         std::string str = std::string(leftImageTopic->GetStringSelection().mb_str());
00171         leftImageSubscriber = nh.subscribe<sensor_msgs::Image>(str, 1, &PTU_GUI::onLeftImage, this);
00172 }
00173 
00174 void PTU_GUI::OnRightTopicChoice( wxCommandEvent& event ) {
00175         rightImageSubscriber.shutdown();
00176         std::string str = std::string(rightImageTopic->GetStringSelection().mb_str());
00177         rightImageSubscriber = nh.subscribe<sensor_msgs::Image>(str, 1, &PTU_GUI::onRightImage, this);
00178 }
00179 
00180 void PTU_GUI::OnPTUChoice( wxCommandEvent& event ) {
00181         std::string new_ptu_name = std::string(ptuChoice->GetStringSelection().mb_str());
00182         if (new_ptu_name.size() == 0) return;
00183         else ptu_name = new_ptu_name;
00184         updatePTUInfo();
00185 }
00186 
00187 void PTU_GUI::OnPanScroll( wxScrollEvent& event ) {
00188         panSpinner->SetValue(panSlider->GetValue());
00189 }
00190 
00191 void PTU_GUI::OnPanSpin( wxSpinEvent& event ) {
00192         panSlider->SetValue(panSpinner->GetValue());
00193 }
00194 
00195 void PTU_GUI::OnPanSpinText( wxCommandEvent& event ) {
00196         panSlider->SetValue(panSpinner->GetValue());
00197 }
00198 
00199 void PTU_GUI::OnTiltScroll( wxScrollEvent& event ) {
00200         tiltSpinner->SetValue(tiltSlider->GetValue());
00201 }
00202 
00203 void PTU_GUI::OnTiltSpin( wxSpinEvent& event ) {
00204         tiltSlider->SetValue(tiltSpinner->GetValue());
00205 }
00206 
00207 void PTU_GUI::OnTiltSpinText( wxCommandEvent& event ) {
00208         tiltSlider->SetValue(tiltSpinner->GetValue());
00209 }
00210 
00211 void PTU_GUI::OnImmChecked( wxCommandEvent& event ) {
00212         immUpdate = immCheck->IsChecked();
00213 }
00214 
00215 void PTU_GUI::OnListenChecked( wxCommandEvent& event ) {
00216         listenForUpdates = listenCheck->IsChecked();
00217         if (listenForUpdates) immCheck->SetValue(false);
00218         immCheck->Enable(!listenForUpdates);
00219         updateButton->Enable(!listenForUpdates);
00220 }
00221 
00222 void PTU_GUI::OnUpdateClicked( wxCommandEvent& event ) {
00223         if (ptu_name.size() == 0) return;
00224     ptu.setParam("asr_flir_ptu_driver/pan_min_angle", (double) pan_min->GetValue());
00225     ptu.setParam("asr_flir_ptu_driver/pan_max_angle", (double) pan_max->GetValue());
00226     ptu.setParam("asr_flir_ptu_driver/pan_base_speed", pan_base->GetValue());
00227     ptu.setParam("asr_flir_ptu_driver/pan_target_speed", pan_target->GetValue());
00228     ptu.setParam("asr_flir_ptu_driver/pan_upper_speed", pan_upper->GetValue());
00229     ptu.setParam("asr_flir_ptu_driver/pan_accel", pan_accel->GetValue());
00230     ptu.setParam("asr_flir_ptu_driver/pan_hold", pan_hold->GetValue());
00231     ptu.setParam("asr_flir_ptu_driver/pan_move", pan_move->GetValue());
00232     ptu.setParam("asr_flir_ptu_driver/tilt_min_angle", (double) tilt_min->GetValue());
00233     ptu.setParam("asr_flir_ptu_driver/tilt_max_angle", (double) tilt_max->GetValue());
00234     ptu.setParam("asr_flir_ptu_driver/tilt_base_speed", tilt_base->GetValue());
00235     ptu.setParam("asr_flir_ptu_driver/tilt_target_speed", tilt_target->GetValue());
00236     ptu.setParam("asr_flir_ptu_driver/tilt_upper_speed", tilt_upper->GetValue());
00237     ptu.setParam("asr_flir_ptu_driver/tilt_accel", tilt_accel->GetValue());
00238     ptu.setParam("asr_flir_ptu_driver/tilt_hold", tilt_hold->GetValue());
00239     ptu.setParam("asr_flir_ptu_driver/tilt_move", tilt_move->GetValue());
00240         std_srvs::Empty empty;
00241         updater.call(empty);
00242 
00243     //############### Überprüfen ob funktioniert, z.B. indem man Wert übergibt der außerhalb der phys. Limits liegt ###############################
00244     updateSlidersFromParamServer();
00245     //#############################################################################################################################################
00246 
00247         if (!immUpdate) {
00248         sensor_msgs::JointState joint_state;
00249         asr_flir_ptu_driver::State msg;
00250         if(use_path_prediction) {
00251             asr_flir_ptu_driver::Predict end_point_prediction;
00252             end_point_prediction.request.pan = panSlider->GetValue();
00253             end_point_prediction.request.tilt = tiltSlider->GetValue();
00254             predict_client.call(end_point_prediction);
00255             joint_state = createJointCommand(end_point_prediction.response.new_pan, end_point_prediction.response.new_tilt, 0, 0);
00256             msg.no_check_forbidden_area = true;
00257         }
00258         else {
00259             joint_state = createJointCommand(panSlider->GetValue(), tiltSlider->GetValue(), 0, 0);
00260             msg.no_check_forbidden_area = false;
00261         }
00262         msg.state = joint_state;
00263         seq_num++;
00264         msg.seq_num = seq_num;
00265         jointStatePublisher.publish(msg);
00266         }
00267 
00268 }
00269 
00270 void PTU_GUI::onStateCommand(const asr_flir_ptu_driver::State::ConstPtr& msg) {
00271         if (!listenForUpdates) return;
00272 
00273     panSpinner->SetValue(msg->state.position[0]);
00274         OnPanSpin(((wxSpinEvent&)wxEVT_NULL));
00275     tiltSpinner->SetValue(msg->state.position[1]);
00276         OnTiltSpin(((wxSpinEvent&)wxEVT_NULL));
00277 }
00278 
00279 void PTU_GUI::onUpdate(wxTimerEvent& evt) {
00280         if (immUpdate) {
00281                 sensor_msgs::JointState joint_state = createJointCommand(panSlider->GetValue(), tiltSlider->GetValue(), 0, 0);
00282         asr_flir_ptu_driver::State msg;
00283         msg.state = joint_state;
00284         seq_num++;
00285         msg.seq_num = seq_num;
00286         jointStatePublisher.publish(msg);
00287         }
00288 
00289         ros::spinOnce();
00290         if (!ros::ok())
00291         {
00292                 Close();
00293         }
00294 }
00295 
00296 wxBitmap* PTU_GUI::createBitmap(const sensor_msgs::Image::ConstPtr& msg) {
00297         //not a particularly good approach; ignores encoding/image size/etc.
00298 //      wxImage* img = new wxImage(msg->width / 2, msg->height / 2);
00299         int sizeMod = msg->width / 640;
00300         wxImage img(msg->width / sizeMod, msg->height / sizeMod);
00301         int bpp = msg->step / msg->width;
00302 
00303         for (int i = 0; i < img.GetHeight(); i++) {
00304                 for (int j = 0; j < img.GetWidth(); j++) {
00305                         img.SetRGB(j, i,
00306                                         msg->data[sizeMod * i * msg->width * bpp + sizeMod * bpp * j + 2],
00307                                         msg->data[sizeMod * i * msg->width * bpp + sizeMod * bpp * j + 1],
00308                                         msg->data[sizeMod * i * msg->width * bpp + sizeMod * bpp * j]);
00309                 }
00310         }
00311 
00312         wxBitmap* image = new wxBitmap(img);
00313         return image;
00314 }
00315 
00316 void PTU_GUI::onLeftImage(const sensor_msgs::Image::ConstPtr& msg) {
00317         leftPanel->setImage(createBitmap(msg));
00318         leftPanel->paintNow();
00319 }
00320 
00321 void PTU_GUI::onRightImage(const sensor_msgs::Image::ConstPtr& msg) {
00322         rightPanel->setImage(createBitmap(msg));
00323         rightPanel->paintNow();
00324 }
00325 
00326 sensor_msgs::JointState PTU_GUI::createJointCommand(double pan, double tilt, double panSpeed, double tiltSpeed) {
00327         sensor_msgs::JointState joint_state;
00328         joint_state.header.stamp = ros::Time::now();
00329         joint_state.name.push_back("pan");
00330         joint_state.position.push_back(pan);
00331         joint_state.velocity.push_back(panSpeed);
00332         joint_state.name.push_back("tilt");
00333         joint_state.position.push_back(tilt);
00334         joint_state.velocity.push_back(tiltSpeed);
00335         return joint_state;
00336 }
00337 
00338 }