40 #include <QMouseEvent> 43 #include <QStaticText> 60 MoveBasePlugin::MoveBasePlugin() :
61 config_widget_(new QWidget()),
63 is_mouse_down_(false),
64 move_base_client_(
"move_base", true),
65 monitoring_action_state_(false)
73 p.setColor(QPalette::Background, Qt::white);
77 ui_.status->setText(
"OK");
78 QPalette p3(
ui_.status->palette());
79 p3.setColor(QPalette::Text, Qt::green);
80 ui_.status->setPalette(p3);
82 QObject::connect(
ui_.pushButtonInitialPose, &QPushButton::toggled,
85 QObject::connect(
ui_.pushButtonGoalPose, &QPushButton::toggled,
88 QObject::connect(
ui_.pushButtonAbort, &QPushButton::clicked,
134 switch (event->type())
136 case QEvent::MouseButtonPress:
138 case QEvent::MouseButtonRelease:
140 case QEvent::MouseMove:
150 ui_.pushButtonAbort->setEnabled( connected );
151 ui_.pushButtonGoalPose->setEnabled( connected );
152 ui_.pushButtonInitialPose->setEnabled( connected );
180 monitoring_action_state_ =
false;
187 monitoring_action_state_ =
false;
196 bool init_checked =
ui_.pushButtonInitialPose->isChecked();
197 bool goal_checked =
ui_.pushButtonGoalPose->isChecked();
198 if( !init_checked && !goal_checked)
203 if (event->button() == Qt::LeftButton)
233 bool init_checked =
ui_.pushButtonInitialPose->isChecked();
234 bool goal_checked =
ui_.pushButtonGoalPose->isChecked();
235 if( !init_checked && !goal_checked)
250 geometry_msgs::Pose& pose =
move_base_msg_.action_goal.goal.target_pose.pose;
253 pose.position.z = 0.0;
257 ui_.pushButtonGoalPose->setChecked(
false);
261 geometry_msgs::PoseWithCovarianceStamped initpose;
266 initpose.pose.pose.position.z = 0.0;
270 ui_.pushButtonInitialPose->setChecked(
false);
278 constexpr QPointF arrow_points[7] = {
279 {10,0}, {6, -2.5}, {6.5,-1}, {0,-1}, {0,1}, {6.5, 1}, {6,2.5}
284 QPointF transformed_points[7];
285 for (
size_t i=0; i<7; i++ )
287 tf::Vector3 point(arrow_points[i].x(), arrow_points[i].y(), 0);
293 glColor3f(0.1, 0.9, 0.1);
295 glBegin(GL_TRIANGLE_FAN);
296 for (
const QPointF& point: transformed_points )
298 glVertex2d(point.x(), point.y());
302 glColor3f(0.0, 0.6, 0.0);
303 glBegin(GL_LINE_LOOP);
304 for (
const QPointF& point: transformed_points )
306 glVertex2d(point.x(), point.y());
325 const bool other_checked =
ui_.pushButtonGoalPose->isChecked();
329 ui_.pushButtonGoalPose->setChecked(
false);
332 QPixmap cursor_pixmap = QPixmap(
":/images/green-arrow.png");
333 QApplication::setOverrideCursor(QCursor(cursor_pixmap));
336 if( !checked && !other_checked )
338 QApplication::restoreOverrideCursor();
344 const bool other_checked =
ui_.pushButtonInitialPose->isChecked();
347 ui_.pushButtonInitialPose->setChecked(
false);
350 QPixmap cursor_pixmap = QPixmap(
":/images/green-arrow.png");
351 QApplication::setOverrideCursor(QCursor(cursor_pixmap));
354 if( !checked && !other_checked )
356 QApplication::restoreOverrideCursor();
move_base_msgs::MoveBaseAction move_base_msg_
void on_pushButtonAbort_clicked()
bool handleMouseMove(QMouseEvent *)
QPointF MapGlCoordToFixedFrame(const QPointF &point)
mapviz::MapCanvas * map_canvas_
QWidget * GetConfigWidget(QWidget *parent)
void publish(const boost::shared_ptr< M > &message) const
virtual bool eventFilter(QObject *object, QEvent *event) override
void SaveConfig(YAML::Emitter &emitter, const std::string &path)
virtual void PrintError(const std::string &message) override
void timerCallback(const ros::TimerEvent &ev=ros::TimerEvent())
bool monitoring_action_state_
static void PrintWarningHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
static void PrintErrorHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
void Draw(double x, double y, double scale)
std::string target_frame_
static void PrintInfoHelper(QLabel *status_label, const std::string &message, double throttle=0.0)
MoveBaseClient move_base_client_
bool handleMousePress(QMouseEvent *)
bool isServerConnected() const
std::string toString() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
static Quaternion createQuaternionFromYaw(double yaw)
void on_pushButtonInitialPose_toggled(bool checked)
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
TFSIMD_FORCE_INLINE const tfScalar & y() const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
QPointF arrow_tail_position_
ros::Publisher init_pose_pub_
virtual void PrintInfo(const std::string &message) override
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
virtual void PrintWarning(const std::string &message) override
bool Initialize(QGLWidget *canvas)
void on_pushButtonGoalPose_toggled(bool checked)
SimpleClientGoalState getState() const
bool handleMouseRelease(QMouseEvent *)
void LoadConfig(const YAML::Node &node, const std::string &path)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
virtual ~MoveBasePlugin()