Esempio n. 1
0
    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)
Esempio n. 2
0
    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")
Esempio n. 3
0
    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))