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
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
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
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
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
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
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
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
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
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 }
00397
00398 #include <pluginlib/class_list_macros.h>
00399 PLUGINLIB_EXPORT_CLASS(jsk_interactive_marker::TransformableMarkerOperatorAction, rviz::Panel )