def add_topic(self, topic_name):
if topic_name in self._rosdata:
qWarning("MatPlot.add_topic(): topic already subscribed: %s" % topic_name)
return
self._rosdata[topic_name] = ROSData(topic_name, self._start_time)
data_x, data_y = self._rosdata[topic_name].next()
color = self.data_plot._colors[self.comboBox.currentText()]
self.data_plot.add_curve(topic_name, data_x, data_y, color)
def _handle_start_following(self):
#make sure server is available before sending goal
QApplication.setOverrideCursor(QCursor(Qt.WaitCursor))
if (not self._client.wait_for_server(rospy.Duration.from_sec(10))):
QApplication.restoreOverrideCursor()
msg_box = QMessageBox()
msg_box.setText("Timeout while looking for path following control server.")
msg_box.exec_()
return
QApplication.restoreOverrideCursor()
#get path file to load
filename = QFileDialog.getOpenFileName(self, self.tr('Load Path from File'), '.', self.tr('Path File {.txt} (*.txt)'))
if filename[0] != '':
try:
handle = open(filename[0])
except IOError as e:
qWarning(str(e))
return
# load path from file
path_reader=csv.reader(open(filename[0],'rb'),delimiter=';')
# parse path file and convert to numbers
temp = [row[0].split() for row in path_reader]
lines= [[float(num) for num in row] for row in temp]
# first line of file is radius
radius = lines[0]
# remove radius
del(lines[0])
now = rospy.Time.now()
goal = auxos_messages.msg.PlanThenFollowDubinsPathGoal()
goal.path.header.stamp = now
goal.path.header.frame_id = self.path_frame_id # TODO: fix this!!!
for line in lines:
pose_msg = PoseStamped()
pose_msg.header.stamp = now
pose_msg.header.frame_id = goal.path.header.frame_id
pose_msg.pose.position.x = line[0]
pose_msg.pose.position.y = line[1]
quat = qfe(0, 0, psi2theta(line[2]))
pose_msg.pose.orientation.x = quat[0]
pose_msg.pose.orientation.y = quat[1]
pose_msg.pose.orientation.z = quat[2]
pose_msg.pose.orientation.w = quat[3]
goal.path.poses.append(pose_msg)
self._client.send_goal(goal, self._handle_path_complete, self._handle_active, self._handle_feedback)
print("start following")
def dragEnterEvent(self, event):
if not event.mimeData().hasText():
if not hasattr(event.source(), "selectedItems") or len(event.source().selectedItems()) == 0:
qWarning(
"MatPlot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0"
)
return
item = event.source().selectedItems()[0]
ros_topic_name = item.data(0, Qt.UserRole)
if ros_topic_name == None:
qWarning("MatPlot.dragEnterEvent(): not hasattr(item, ros_topic_name_)")
return
# get topic name
if event.mimeData().hasText():
topic_name = str(event.mimeData().text())
else:
droped_item = event.source().selectedItems()[0]
topic_name = str(droped_item.data(0, Qt.UserRole))
# check for numeric field type
is_numeric, message = is_slot_numeric(topic_name)
if is_numeric:
event.acceptProposedAction()
else:
qWarning('MatPlot.dragEnterEvent(): rejecting: "%s"' % (message))
