simulation_widget.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Mohamad Ayman.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * The name of Mohamad Ayman may not be used to endorse or promote products derived
18  * from this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *********************************************************************/
33 
34 /* Author: Mohamad Ayman */
35 
36 // SA
37 #include "simulation_widget.h"
38 
39 // Qt
40 #include <QVBoxLayout>
41 #include <QMessageBox>
42 #include <QPushButton>
43 #include <QColor>
44 
46 #include <moveit_msgs/DisplayRobotState.h>
47 
48 #include <regex>
49 
50 namespace moveit_setup_assistant
51 {
52 // ******************************************************************************************
53 // Constructor
54 // ******************************************************************************************
55 SimulationWidget::SimulationWidget(QWidget* parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
56  : SetupScreenWidget(parent), config_data_(config_data)
57 {
58  // Basic widget container
59  QVBoxLayout* layout = new QVBoxLayout();
60  layout->setAlignment(Qt::AlignTop);
61 
62  // Top Header Area ------------------------------------------------
63 
64  HeaderWidget* header =
65  new HeaderWidget("Simulate With Gazebo", "The following tool will auto-generate the URDF changes needed "
66  "for Gazebo compatibility with ROSControl and MoveIt!. The "
67  "needed changes are shown in green.",
68  this);
69  layout->addWidget(header);
70 
71  // Spacing
72  QSpacerItem* blank_space = new QSpacerItem(1, 8);
73  layout->addSpacerItem(blank_space);
74 
75  QLabel* instructions = new QLabel(this);
76  instructions->setText("You can run the following command to quickly find the necessary URDF file to edit:");
77  layout->addWidget(instructions);
78 
79  QTextEdit* instructions_command = new QTextEdit(this);
80  instructions_command->setText(std::string("roscd " + config_data->urdf_pkg_name_).c_str());
81  instructions_command->setReadOnly(true);
82  instructions_command->setMaximumHeight(30);
83  layout->addWidget(instructions_command);
84 
85  // Spacing
86  blank_space = new QSpacerItem(1, 6);
87  layout->addSpacerItem(blank_space);
88 
89  // Used to make the new URDF visible
90  QPushButton* btn_generate = new QPushButton("&Generate URDF", this);
91  btn_generate->setMinimumWidth(180);
92  btn_generate->setMinimumHeight(40);
93  layout->addWidget(btn_generate);
94  layout->setAlignment(btn_generate, Qt::AlignLeft);
95  connect(btn_generate, SIGNAL(clicked()), this, SLOT(generateURDFClick()));
96 
97  // When there wa no change to be made
98  no_changes_label_ = new QLabel(this);
99  no_changes_label_->setText("No Changes To Be Made");
100  layout->addWidget(no_changes_label_);
101  no_changes_label_->setVisible(false);
102 
103  // URDF text
104  simulation_text_ = new QTextEdit(this);
105  simulation_text_->setLineWrapMode(QTextEdit::NoWrap);
106  layout->addWidget(simulation_text_);
107 
108  // Copy URDF link, hidden initially
109  copy_urdf_ = new QLabel(this);
110  copy_urdf_->setText("<a href='contract'>Copy to Clipboard</a>");
111  connect(copy_urdf_, SIGNAL(linkActivated(const QString)), this, SLOT(copyURDF(const QString)));
112  copy_urdf_->setVisible(false);
113  layout->addWidget(copy_urdf_);
114 
115  // Finish Layout --------------------------------------------------
116  this->setLayout(layout);
117 }
118 
119 // ******************************************************************************************
120 // Called when generate URDF button is clicked
121 // ******************************************************************************************
123 {
124  simulation_text_->setVisible(true);
125  std::string gazebo_compatible_urdf_string = config_data_->getGazeboCompatibleURDF();
126  std::size_t urdf_length = gazebo_compatible_urdf_string.length();
127 
128  // Check if the urdf do need new elements to be added
129  if (urdf_length > 0)
130  {
131  // Split the added elements from the original urdf to view them in different colors
132  std::smatch start_match;
133  std::smatch end_match;
134  std::regex start_reg_ex("<inertial");
135  std::regex end_reg_ex("</inertial");
136 
137  // Search for inertial elemnts using regex
138  std::regex_search(gazebo_compatible_urdf_string, start_match, start_reg_ex);
139  std::regex_search(gazebo_compatible_urdf_string, end_match, end_reg_ex);
140 
141  // Used to cache the positions of the opening and closing of the inertial elements
142  std::vector<int> inertial_opening_matches;
143  std::vector<int> inertial_closing_matches;
144 
145  inertial_closing_matches.push_back(0);
146 
147  // Cache the positions of the openings of the inertial elements
148  for (auto it = std::sregex_iterator(gazebo_compatible_urdf_string.begin(), gazebo_compatible_urdf_string.end(),
149  start_reg_ex);
150  it != std::sregex_iterator(); ++it)
151  {
152  inertial_opening_matches.push_back(it->position());
153  }
154 
155  inertial_opening_matches.push_back(urdf_length);
156 
157  // Cache the positions of the closings of the inertial elements
158  for (auto it = std::sregex_iterator(gazebo_compatible_urdf_string.begin(), gazebo_compatible_urdf_string.end(),
159  end_reg_ex);
160  it != std::sregex_iterator(); ++it)
161  {
162  inertial_closing_matches.push_back(it->position());
163  }
164 
165  for (std::size_t match_number = 0; match_number < inertial_opening_matches.size() - 1; match_number++)
166  {
167  // Show the unmodified elements in black
168  simulation_text_->setTextColor(QColor("black"));
169  simulation_text_->append(
170  QString(gazebo_compatible_urdf_string
171  .substr(inertial_closing_matches[match_number],
172  inertial_opening_matches[match_number] - inertial_closing_matches[match_number])
173  .c_str()));
174 
175  // Show the added elements in green
176  simulation_text_->setTextColor(QColor("green"));
177 
178  simulation_text_->append(
179  QString(gazebo_compatible_urdf_string
180  .substr(inertial_opening_matches[match_number],
181  inertial_closing_matches[match_number + 1] - inertial_opening_matches[match_number] + 11)
182  .c_str()));
183  inertial_closing_matches[match_number + 1] += 11;
184  }
185 
186  // Position of the first transmission element in the urdf
187  std::size_t first_transmission = gazebo_compatible_urdf_string.find("<transmission");
188 
189  // Position of the last inertial element in the urdf
190  std::size_t last_inertial = inertial_closing_matches[inertial_opening_matches.size() - 1];
191 
192  if (first_transmission != std::string::npos)
193  {
194  simulation_text_->setTextColor(QColor("black"));
195  simulation_text_->append(
196  QString(gazebo_compatible_urdf_string.substr(last_inertial, first_transmission - last_inertial).c_str()));
197 
198  // Write from the first transmission element until the closing robot element in green
199  simulation_text_->setTextColor(QColor("green"));
200  simulation_text_->append(QString(
201  gazebo_compatible_urdf_string.substr(first_transmission, urdf_length - first_transmission - 10).c_str()));
202 
203  // Write the closing robot element in black
204  simulation_text_->setTextColor(QColor("black"));
205  simulation_text_->append(QString("</robot>"));
206  }
207 
208  // Copy link appears after the text is ready
209  copy_urdf_->setVisible(true);
210  }
211  else
212  {
213  simulation_text_->append(QString(gazebo_compatible_urdf_string.c_str()));
214  no_changes_label_->setVisible(true);
215  }
216 }
217 
218 // ******************************************************************************************
219 // Called the copy to clipboard button is clicked
220 // ******************************************************************************************
221 void SimulationWidget::copyURDF(const QString& link)
222 {
223  simulation_text_->selectAll();
224  simulation_text_->copy();
225 }
226 
227 } // namespace
SimulationWidget(QWidget *parent, moveit_setup_assistant::MoveItConfigDataPtr config_data)
moveit_setup_assistant::MoveItConfigDataPtr config_data_
Contains all the configuration data for the setup assistant.
void generateURDFClick()
Generate URDF button clicked.


moveit_setup_assistant
Author(s): Dave Coleman
autogenerated on Sun Oct 18 2020 13:19:28