motion_planning_frame_joints_widget.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, CITEC, Bielefeld University
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of CITEC / Bielefeld University nor the names of
 *     its contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
/* Author: Robert Haschke */
 
#pragma once
 
#include <moveit/macros/class_forward.h>
#include <moveit/robot_state/robot_state.h>
#include <Eigen/SVD>
#include <QAbstractItemModel>
#include <QWidget>
#include <QStyledItemDelegate>
#include <vector>
#include <memory>
 
class QSlider;
 
namespace Ui
{
class MotionPlanningFrameJointsUI;
}
namespace robot_interaction
{
MOVEIT_CLASS_FORWARD(InteractionHandler);
}
namespace moveit_rviz_plugin
{
class JMGItemModel : public QAbstractTableModel
{
  Q_OBJECT
  moveit::core::RobotState robot_state_;
  const moveit::core::JointModelGroup* jmg_;
 
public:
  JMGItemModel(const moveit::core::RobotState& robot_state, const std::string& group_name, QObject* parent = nullptr);
 
  // QAbstractItemModel interface
  int rowCount(const QModelIndex& parent = QModelIndex()) const override;
  int columnCount(const QModelIndex& parent = QModelIndex()) const override;
  Qt::ItemFlags flags(const QModelIndex& index) const override;
  QVariant data(const QModelIndex& index, int role) const override;
  QVariant headerData(int section, Qt::Orientation orientation, int role) const override;
  bool setData(const QModelIndex& index, const QVariant& value, int role) override;
 
  void updateRobotState(const moveit::core::RobotState& state);
 
  moveit::core::RobotState& getRobotState()
  {
    return robot_state_;
  }
  const moveit::core::RobotState& getRobotState() const
  {
    return robot_state_;
  }
  const moveit::core::JointModelGroup* getJointModelGroup() const
  {
    return jmg_;
  }
 
private:
  const moveit::core::JointModel* getJointModel(const QModelIndex& index) const;
  const moveit::core::VariableBounds* getVariableBounds(const moveit::core::JointModel* jm,
                                                        const QModelIndex& index) const;
};
 
class JointsWidgetEventFilter : public QObject
{
  Q_OBJECT
  QModelIndex active_;  // joint index being operated on
  int pmin_, pmax_;     // pixel min/max values
  float delta_ = 0.0f;  // speed of joint value changes from keyboard interaction
 
public:
  JointsWidgetEventFilter(QAbstractItemView* view);
 
protected:
  bool eventFilter(QObject* target, QEvent* event) override;
};
 
class MotionPlanningDisplay;
class MotionPlanningFrameJointsWidget : public QWidget
{
  Q_OBJECT
 
public:
  MotionPlanningFrameJointsWidget(const MotionPlanningFrameJointsWidget&) = delete;
  MotionPlanningFrameJointsWidget(MotionPlanningDisplay* display, QWidget* parent = nullptr);
  ~MotionPlanningFrameJointsWidget() override;
 
  void clearRobotModel();
  void changePlanningGroup(const std::string& group_name,
                           const robot_interaction::InteractionHandlerPtr& start_state_handler,
                           const robot_interaction::InteractionHandlerPtr& goal_state_handler);
 
  bool useRadians() const;
  void setUseRadians(bool use_radians);
 
Q_SIGNALS:
  void configChanged();
 
public Q_SLOTS:
  void queryStartStateChanged();
  void queryGoalStateChanged();
  void jogNullspace(double value);
 
protected:
  void setActiveModel(JMGItemModel* model);
  void triggerUpdate(JMGItemModel* model);
  void updateNullspaceSliders();
  QSlider* createNSSlider(int i);
 
private:
  Ui::MotionPlanningFrameJointsUI* ui_;
  MotionPlanningDisplay* planning_display_;
  robot_interaction::InteractionHandlerPtr start_state_handler_;
  robot_interaction::InteractionHandlerPtr goal_state_handler_;
  std::unique_ptr<JMGItemModel> start_state_model_;
  std::unique_ptr<JMGItemModel> goal_state_model_;
  // break circular loop of stateChanged() -> dataChanged() |-> PlanningDisplay::setQuery*State()
  bool ignore_state_changes_ = false;
 
  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
  Eigen::MatrixXd nullspace_;
  std::vector<QSlider*> ns_sliders_;
};
 
class ProgressBarDelegate : public QStyledItemDelegate
{
  Q_OBJECT
 
public:
  enum CustomRole
  {
    JointTypeRole = Qt::UserRole,  // NOLINT(readability-identifier-naming)
    VariableBoundsRole,            // NOLINT(readability-identifier-naming)
    JointRangeFractionRole,        // NOLINT(readability-identifier-naming)
  };
  enum RevoluteUnit
  {
    DEGREES = 0,
    RADIANS = 1,
  };
 
  ProgressBarDelegate(QWidget* parent = nullptr) : QStyledItemDelegate(parent), unit_(DEGREES)
  {
  }
 
  void setUnit(RevoluteUnit unit)
  {
    unit_ = unit;
  }
  void paint(QPainter* painter, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
  QWidget* createEditor(QWidget* parent, const QStyleOptionViewItem& option, const QModelIndex& index) const override;
  bool isEditing() const;
  void setEditorData(QWidget* editor, const QModelIndex& index) const override;
  void setModelData(QWidget* editor, QAbstractItemModel* model, const QModelIndex& index) const override;
 
  RevoluteUnit unit_;
 
protected Q_SLOTS:
  void onEditorDestroyed(QObject* /* editor */) const;
 
private:
  mutable int editor_open_count_ = 0;
};
 
class JogSlider : public QSlider
{
  Q_OBJECT
  int timer_id_;
  int timer_interval_;  // ms
  double maximum_;
 
public:
  JogSlider(QWidget* parent = nullptr);
 
  int timerInterval() const
  {
    return timer_interval_;
  }
  void setTimerInterval(int ms);
  void setResolution(unsigned int resolution);
  void setMaximum(double value);
  double value() const
  {
    return QSlider::value() * maximum_ / QSlider::maximum();
  }
 
protected:
  void timerEvent(QTimerEvent* event) override;
  void mousePressEvent(QMouseEvent* event) override;
  void mouseReleaseEvent(QMouseEvent* event) override;
 
private:
  using QSlider::setMaximum;
  using QSlider::setMinimum;
  using QSlider::setRange;
 
Q_SIGNALS:
  void triggered(double value);
};
}  // namespace moveit_rviz_plugin