jsk_interactive_marker: transformable_marker

Go to the documentation of this file.
00001 #include <iostream>
00002 
00003 #include <QLineEdit>
00004 #include <QPushButton>
00005 #include <QVBoxLayout>
00006 #include <QHBoxLayout>
00007 #include <QTabWidget>
00008 #include <QLabel>
00009 
00010 #include <rviz/visualization_manager.h>
00011 #include <rviz/frame_manager.h>
00012 
00013 #include <resource_retriever/retriever.h>
00014 #include <jsk_interactive_marker/GetMarkerDimensions.h>
00015 #include <jsk_interactive_marker/GetTransformableMarkerFocus.h>
00016 
00017 #include "jsk_interactive_marker/rviz_plugins/transformable_marker_operator.h"
00018 
00019 using namespace rviz;
00020 namespace jsk_interactive_marker
00021 {
00022   TransformableMarkerOperatorAction::TransformableMarkerOperatorAction( QWidget* parent )
00023     : rviz::Panel( parent )
00024   {
00025     layout = new QVBoxLayout;
00026 
00027     // server name
00028     QHBoxLayout* server_name_layout = new QHBoxLayout;
00029     server_name_layout->addWidget( new QLabel( "Server Name:" ));
00030     server_name_editor_ = new QLineEdit;
00031     server_name_layout->addWidget( server_name_editor_ );
00032     layout->addLayout( server_name_layout );
00033 
00034     // topic name
00035     QHBoxLayout* obj_array_topic_layout = new QHBoxLayout;
00036     obj_array_topic_layout->addWidget( new QLabel( "ObjectArray Topic:" ));
00037     topic_name_editor_ = new QLineEdit;
00038     obj_array_topic_layout->addWidget( topic_name_editor_ );
00039     layout->addLayout( obj_array_topic_layout );
00040 
00041     // tabs for operations
00042     QTabWidget* tabs = new QTabWidget();
00043 
00044     QVBoxLayout* layout1 = new QVBoxLayout;
00045     QVBoxLayout* layout2 = new QVBoxLayout;
00046     QVBoxLayout* layout3 = new QVBoxLayout;
00047 
00048     QWidget* tab_1 = new QWidget();
00049     QWidget* tab_2 = new QWidget();
00050     QWidget* tab_3 = new QWidget();
00051 
00052     // tab_1, layout1: insert
00053 
00054     insert_box_button_ = new QPushButton("Insert New Box Marker");
00055     layout1->addWidget( insert_box_button_ );
00056 
00057     insert_cylinder_button_ = new QPushButton("Insert New Cylinder Marker");
00058     layout1->addWidget( insert_cylinder_button_ );
00059 
00060     insert_torus_button_ = new QPushButton("Insert New Torus Marker");
00061     layout1->addWidget( insert_torus_button_ );
00062 
00063     insert_mesh_button_ = new QPushButton("Insert New Mesh Marker");
00064     layout1->addWidget( insert_mesh_button_ );
00065 
00066     QHBoxLayout* object_layout = new QHBoxLayout;
00067     object_layout->addWidget( new QLabel( "Object:" ));
00068     object_editor_ = new QComboBox;
00069     object_editor_->setIconSize(QSize(50, 50));
00070     object_editor_->setSizeAdjustPolicy(QComboBox::AdjustToContents);
00071     object_layout->addWidget( object_editor_ );
00072     layout1->addLayout( object_layout );
00073 
00074     QHBoxLayout* name_layout = new QHBoxLayout;
00075     name_layout->addWidget( new QLabel( "Name:" ));
00076     name_editor_ = new QLineEdit;
00077     name_layout->addWidget( name_editor_ );
00078     layout1->addLayout( name_layout );
00079 
00080     QHBoxLayout* description_layout = new QHBoxLayout;
00081     description_layout->addWidget( new QLabel( "Description:" ));
00082     description_editor_ = new QLineEdit;
00083     description_layout->addWidget( description_editor_ );
00084     layout1->addLayout( description_layout );
00085 
00086     QHBoxLayout* frame_layout = new QHBoxLayout;
00087     frame_layout->addWidget( new QLabel( "Frame:" ));
00088     frame_editor_ = new QLineEdit;
00089     frame_layout->addWidget( frame_editor_ );
00090     layout1->addLayout( frame_layout );
00091 
00092     // tab_2, layout2: transform
00093     QVBoxLayout* transform_layout = new QVBoxLayout;
00094     transform_layout->addWidget( new QLabel( "Object Name:" ));
00095     transform_name_editor_ = new QLineEdit;
00096     transform_layout->addWidget( transform_name_editor_ );
00097     transform_layout->addWidget( new QLabel( "Dimension X:" ));
00098     dimension_x_editor_ = new QLineEdit;
00099     transform_layout->addWidget( dimension_x_editor_ );
00100     transform_layout->addWidget( new QLabel( "Dimension Y:" ));
00101     dimension_y_editor_ = new QLineEdit;
00102     transform_layout->addWidget( dimension_y_editor_ );
00103     transform_layout->addWidget( new QLabel( "Dimension Z:" ));
00104     dimension_z_editor_ = new QLineEdit;
00105     transform_layout->addWidget( dimension_z_editor_ );
00106     transform_layout->addWidget( new QLabel( "Dimension Radius:" ));
00107     dimension_radius_editor_ = new QLineEdit;
00108     transform_layout->addWidget( dimension_radius_editor_ );
00109     transform_layout->addWidget( new QLabel( "Dimension Small Radius:" ));
00110     dimension_sm_radius_editor_ = new QLineEdit;
00111     transform_layout->addWidget( dimension_sm_radius_editor_ );
00112     layout2->addLayout( transform_layout );
00113 
00114     // tab_3, layout3: erase
00115 
00116     erase_with_id_button_ = new QPushButton("Erase with id");
00117     layout3->addWidget( erase_with_id_button_ );
00118 
00119     QHBoxLayout* id_layout = new QHBoxLayout;
00120     id_layout->addWidget( new QLabel( "Id:" ));
00121     id_editor_ = new QLineEdit;
00122     id_layout->addWidget( id_editor_ );
00123     layout3->addLayout( id_layout );
00124 
00125     erase_all_button_ = new QPushButton("Erase all");
00126     layout3->addWidget( erase_all_button_ );
00127 
00128     erase_focus_button_ = new QPushButton("Erase focus");
00129     layout3->addWidget( erase_focus_button_ );
00130 
00131     tab_1->setLayout( layout1 );
00132     tab_2->setLayout( layout2 );
00133     tab_3->setLayout( layout3 );
00134 
00135     tabs->addTab(tab_1, QString("Insert"));
00136     tabs->addTab(tab_2, QString("Transform"));
00137     tabs->addTab(tab_3, QString("Erase"));
00138 
00139     layout->addWidget( tabs );
00140     setLayout( layout );
00141 
00142     connect( insert_box_button_, SIGNAL( clicked() ), this, SLOT( insertBoxService ()));
00143     connect( insert_cylinder_button_, SIGNAL( clicked() ), this, SLOT( insertCylinderService ()));
00144     connect( insert_torus_button_, SIGNAL( clicked() ), this, SLOT( insertTorusService ()));
00145     connect( insert_mesh_button_, SIGNAL( clicked() ), this, SLOT( insertMeshService ()));
00146     connect( erase_with_id_button_, SIGNAL( clicked() ), this, SLOT( eraseWithIdService ()));
00147     connect( erase_all_button_, SIGNAL( clicked() ), this, SLOT( eraseAllService ()));
00148     connect( erase_focus_button_, SIGNAL( clicked() ), this, SLOT( eraseFocusService ()));
00149 
00150     connect( dimension_x_editor_, SIGNAL( editingFinished() ), this, SLOT( updateDimensionsService ()));
00151     connect( dimension_y_editor_, SIGNAL( editingFinished() ), this, SLOT( updateDimensionsService ()));
00152     connect( dimension_z_editor_, SIGNAL( editingFinished() ), this, SLOT( updateDimensionsService ()));
00153     connect( dimension_radius_editor_, SIGNAL( editingFinished() ), this, SLOT( updateDimensionsService ()));
00154     connect( dimension_sm_radius_editor_, SIGNAL( editingFinished() ), this, SLOT( updateDimensionsService ()));
00155     connect( object_editor_, SIGNAL( currentIndexChanged(int)), SLOT( updateName ()));
00156     connect( topic_name_editor_, SIGNAL( editingFinished() ), this, SLOT( updateObjectArrayTopic ()));
00157   }
00158 
00159   void TransformableMarkerOperatorAction::objectArrayCb(const jsk_recognition_msgs::ObjectArray::ConstPtr& obj_array_msg) {
00160     objects_ = obj_array_msg->objects;
00161     int current_index = object_editor_->currentIndex();
00162     object_editor_->clear();
00163     for (size_t i = 0; i < obj_array_msg->objects.size(); i++) {
00164       jsk_recognition_msgs::Object object =  objects_[i];
00165       // thumbnail
00166       QPixmap pixmap = QPixmap();
00167       if (object.image_resources.size() > 0)
00168       {
00169         std::string thumbnail = object.image_resources[0];
00170         resource_retriever::Retriever retriever;
00171         resource_retriever::MemoryResource mem = retriever.get(thumbnail);
00172         pixmap.loadFromData(static_cast<unsigned char*>(mem.data.get()), mem.size);
00173       }
00174       // name
00175       std::stringstream ss;
00176       ss << object.id << ": " << object.name;
00177       //
00178       object_editor_->addItem(QIcon(pixmap), QString::fromStdString(ss.str()));
00179     }
00180     object_editor_->setCurrentIndex(current_index);
00181   }
00182 
00183   void TransformableMarkerOperatorAction::onInitialize() {
00184     connect( vis_manager_, SIGNAL( preUpdate() ), this, SLOT( update() ));
00185     updateObjectArrayTopic();
00186   }
00187 
00188   void TransformableMarkerOperatorAction::update() {
00189     updateServerName();
00190     updateFocusMarkerDimensions();
00191     updateFrameId();
00192     // updateDimensionsService();
00193   }
00194 
00195   void TransformableMarkerOperatorAction::updateObjectArrayTopic() {
00196     sub_obj_array_.shutdown();
00197     std::string topic = topic_name_editor_->text().toStdString();
00198     if (topic.empty()) {
00199       ros::master::V_TopicInfo topics;
00200       ros::master::getTopics(topics);
00201       for (size_t i = 0; i < topics.size(); i++) {
00202         if (topics[i].datatype == "jsk_recognition_msgs/ObjectArray") {
00203           topic = topics[i].name;
00204           break;
00205         }
00206       }
00207       topic_name_editor_->setText(QString::fromStdString(topic));
00208     }
00209     sub_obj_array_ = nh_.subscribe(
00210       topic, 1, &TransformableMarkerOperatorAction::objectArrayCb, this);
00211   }
00212 
00213   void TransformableMarkerOperatorAction::insertBoxService(){
00214     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00215     operator_srv.request.operate.type = jsk_rviz_plugins::TransformableMarkerOperate::BOX;
00216     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::INSERT;
00217     operator_srv.request.operate.name = name_editor_->text().toStdString();
00218     operator_srv.request.operate.description = description_editor_->text().toStdString();
00219     operator_srv.request.operate.frame_id = frame_editor_->text().toStdString();
00220     callRequestMarkerOperateService(operator_srv);
00221   };
00222 
00223   void TransformableMarkerOperatorAction::insertCylinderService(){
00224     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00225     operator_srv.request.operate.type = jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER;
00226     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::INSERT;
00227     operator_srv.request.operate.name = name_editor_->text().toStdString();
00228     operator_srv.request.operate.description = description_editor_->text().toStdString();
00229     operator_srv.request.operate.frame_id = frame_editor_->text().toStdString();
00230     callRequestMarkerOperateService(operator_srv);
00231   };
00232 
00233   void TransformableMarkerOperatorAction::insertMeshService() {
00234     int current_index = object_editor_->currentIndex();
00235     if (!(0 <= current_index && current_index < objects_.size())) {
00236       ROS_ERROR("Invalid index for object selection: %d. Please select again.", current_index);
00237       return;
00238     }
00239     jsk_recognition_msgs::Object object = objects_[current_index];
00240     if (object.mesh_resource.empty()) {
00241       ROS_ERROR("Mesh resource of object '%s' is empty, so skipping.", object.name.c_str());
00242       return;
00243     }
00244 
00245     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00246     operator_srv.request.operate.type = jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE;
00247     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::INSERT;
00248     operator_srv.request.operate.name = object.name;
00249     operator_srv.request.operate.description = description_editor_->text().toStdString();
00250     operator_srv.request.operate.frame_id = frame_editor_->text().toStdString();
00251     operator_srv.request.operate.mesh_resource = object.mesh_resource;
00252     operator_srv.request.operate.mesh_use_embedded_materials = true;
00253     callRequestMarkerOperateService(operator_srv);
00254   };
00255 
00256   void TransformableMarkerOperatorAction::insertTorusService(){
00257     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00258     operator_srv.request.operate.type = jsk_rviz_plugins::TransformableMarkerOperate::TORUS;
00259     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::INSERT;
00260     operator_srv.request.operate.name = name_editor_->text().toStdString();
00261     operator_srv.request.operate.description = description_editor_->text().toStdString();
00262     operator_srv.request.operate.frame_id = frame_editor_->text().toStdString();
00263     callRequestMarkerOperateService(operator_srv);
00264   };
00265 
00266   void TransformableMarkerOperatorAction::eraseWithIdService(){
00267     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00268     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::ERASE;
00269     operator_srv.request.operate.name = id_editor_->text().toStdString();
00270     callRequestMarkerOperateService(operator_srv);
00271   };
00272 
00273   void TransformableMarkerOperatorAction::eraseAllService(){
00274     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00275     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::ERASEALL;
00276     callRequestMarkerOperateService(operator_srv);
00277   };
00278 
00279   void TransformableMarkerOperatorAction::eraseFocusService(){
00280     jsk_rviz_plugins::RequestMarkerOperate operator_srv;
00281     operator_srv.request.operate.action = jsk_rviz_plugins::TransformableMarkerOperate::ERASEFOCUS;
00282     callRequestMarkerOperateService(operator_srv);
00283   };
00284 
00285   void TransformableMarkerOperatorAction::callRequestMarkerOperateService(jsk_rviz_plugins::RequestMarkerOperate srv){
00286     std::string server_name = server_name_editor_->text().toStdString();
00287     std::string service_name = server_name + "/request_marker_operate";
00288     ros::ServiceClient client = nh_.serviceClient<jsk_rviz_plugins::RequestMarkerOperate>(service_name, true);
00289     if(client.call(srv))
00290       {
00291         ROS_INFO("Call Success");
00292       }
00293     else{
00294       ROS_ERROR("Service call FAIL: %s", service_name.c_str());
00295     };
00296   }
00297 
00298   void TransformableMarkerOperatorAction::updateDimensionsService() {
00299     std::string server_name = server_name_editor_->text().toStdString();
00300     std::string service_name = server_name + "/set_dimensions";
00301     ros::ServiceClient client = nh_.serviceClient<jsk_interactive_marker::SetMarkerDimensions>(service_name, true);
00302 
00303     jsk_interactive_marker::SetMarkerDimensions srv;
00304     if (transform_name_editor_->text().toStdString().empty()) {
00305       srv.request.dimensions.x = transform_name_editor_->placeholderText().toFloat();
00306     } else {
00307       srv.request.dimensions.x = transform_name_editor_->text().toFloat();
00308     }
00309     if (dimension_x_editor_->text().toStdString().empty()) {
00310       srv.request.dimensions.x = dimension_x_editor_->placeholderText().toFloat();
00311     } else {
00312       srv.request.dimensions.x = dimension_x_editor_->text().toFloat();
00313     }
00314     if (dimension_y_editor_->text().toStdString().empty()) {
00315       srv.request.dimensions.y = dimension_y_editor_->placeholderText().toFloat();
00316     } else {
00317       srv.request.dimensions.y = dimension_y_editor_->text().toFloat();
00318     }
00319     if (dimension_z_editor_->text().toStdString().empty()) {
00320       srv.request.dimensions.z = dimension_z_editor_->placeholderText().toFloat();
00321     } else {
00322       srv.request.dimensions.z = dimension_z_editor_->text().toFloat();
00323     }
00324 
00325     if (client.call(srv)) {
00326       ROS_INFO("Call success: %s", service_name.c_str());
00327     } else {
00328       ROS_ERROR("Service call fail: %s", service_name.c_str());
00329     }
00330   }
00331 
00332   void TransformableMarkerOperatorAction::updateFrameId() {
00333     if (frame_editor_->text().isEmpty()) {
00334       frame_editor_->setText(vis_manager_->getFixedFrame());
00335     }
00336   }
00337 
00338   void TransformableMarkerOperatorAction::updateName() {
00339     int current_index = object_editor_->currentIndex();
00340     if (0 <= current_index && current_index < objects_.size()) {
00341       jsk_recognition_msgs::Object object = objects_[current_index];
00342       name_editor_->setText(QString::fromStdString(object.name));
00343     } else {
00344       name_editor_->setText(QString(""));
00345     }
00346   }
00347 
00348   void TransformableMarkerOperatorAction::updateServerName() {
00349     std::string server_name = server_name_editor_->text().toStdString();
00350     if (server_name.empty() && !ros::service::exists("/request_marker_operate", false)) {
00351       ros::V_string nodes;
00352       ros::master::getNodes(nodes);
00353       for (size_t i=0; i<nodes.size(); i++) {
00354         if (ros::service::exists(nodes[i] + "/request_marker_operate", false)) {
00355           server_name_editor_->setText(QString::fromStdString(nodes[i]));
00356           break;
00357         }
00358       }
00359     }
00360   }
00361 
00362   void TransformableMarkerOperatorAction::updateFocusMarkerDimensions() {
00363     std::string server_name = server_name_editor_->text().toStdString();
00364     ros::ServiceClient client_focus = nh_.serviceClient<jsk_interactive_marker::GetTransformableMarkerFocus>(
00365       server_name + "/get_focus", true);
00366     jsk_interactive_marker::GetTransformableMarkerFocus srv_focus;
00367     ros::ServiceClient client_dim = nh_.serviceClient<jsk_interactive_marker::GetMarkerDimensions>(
00368       server_name + "/get_dimensions", true);
00369     jsk_interactive_marker::GetMarkerDimensions srv_dim;
00370     if (client_focus.call(srv_focus) && client_dim.call(srv_dim)) {
00371       transform_name_editor_->setPlaceholderText(QString::fromStdString(srv_focus.response.target_name));
00372       dimension_x_editor_->setPlaceholderText(QString::number(srv_dim.response.dimensions.x, 'f', 4));
00373       dimension_y_editor_->setPlaceholderText(QString::number(srv_dim.response.dimensions.y, 'f', 4));
00374       dimension_z_editor_->setPlaceholderText(QString::number(srv_dim.response.dimensions.z, 'f', 4));
00375       dimension_radius_editor_->setPlaceholderText(QString::number(srv_dim.response.dimensions.radius, 'f', 4));
00376       dimension_sm_radius_editor_->setPlaceholderText(
00377         QString::number(srv_dim.response.dimensions.small_radius, 'f', 4));
00378     } else{
00379       ROS_ERROR_THROTTLE(10, "Service call FAIL: %s", server_name.c_str());
00380     }
00381   }
00382 
00383   void TransformableMarkerOperatorAction::save( rviz::Config config ) const
00384   {
00385     rviz::Panel::save( config );
00386     config.mapSetValue( "ServerName", server_name_editor_->text().toStdString().c_str() );
00387   }
00388 
00389   void TransformableMarkerOperatorAction::load( const rviz::Config& config )
00390   {
00391     rviz::Panel::load( config );
00392     QString server_name;
00393     config.mapGetString( "ServerName", &server_name );
00394     server_name_editor_->setText(server_name);
00395   }
00396 }  // namespace jsk_interactive_marker
00397 
00398 #include <pluginlib/class_list_macros.h>
00399 PLUGINLIB_EXPORT_CLASS(jsk_interactive_marker::TransformableMarkerOperatorAction, rviz::Panel )