Fawkes API  Fawkes Development Version
move_base_thread.h
1 
2 /***************************************************************************
3  * move_base_thread.h - Emulate ROS move_base
4  *
5  * Created: Wed May 07 13:48:59 2014
6  * Copyright 2014 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef _ROS_MOVE_BASE_THREAD_H_
23 #define _ROS_MOVE_BASE_THREAD_H_
24 
25 #include <actionlib/server/simple_action_server.h>
26 #include <aspect/blackboard.h>
27 #include <aspect/blocked_timing.h>
28 #include <aspect/configurable.h>
29 #include <aspect/logging.h>
30 #include <aspect/tf.h>
31 #include <core/threading/thread.h>
32 #include <interfaces/NavigatorInterface.h>
33 #include <move_base_msgs/MoveBaseAction.h>
34 #include <move_base_msgs/MoveBaseActionGoal.h>
35 #include <move_base_msgs/MoveBaseGoal.h>
36 #include <plugins/ros/aspect/ros.h>
37 #include <ros/ros.h>
38 #include <tf/types.h>
39 
40 #include <cmath>
41 
42 namespace fawkes {
43 class NavigatorInterface;
44 }
45 
48  public fawkes::LoggingAspect,
52  public fawkes::ROSAspect
53 {
54 public:
56 
57  /* thread */
58  virtual void init();
59  virtual void finalize();
60  virtual void once();
61  virtual void loop();
62 
63 private:
64  typedef enum { NAVGRAPH, COLLI } ExecType;
65 
66  typedef actionlib::ActionServer<move_base_msgs::MoveBaseAction> MoveBaseServer;
67 
68  void action_goal_cb(MoveBaseServer::GoalHandle goal, ExecType ext);
69  void action_cancel_cb(MoveBaseServer::GoalHandle goal);
70  void message_cb(geometry_msgs::PoseStampedConstPtr goal_pose, ExecType ext);
71 
72  void stop();
73  move_base_msgs::MoveBaseResult create_result();
74  move_base_msgs::MoveBaseFeedback create_feedback();
75 
76 private:
77  fawkes::NavigatorInterface *nav_navgraph_if_;
78  fawkes::NavigatorInterface *nav_colli_if_;
80 
81  MoveBaseServer *as_colli_;
82  MoveBaseServer *as_navgraph_;
83  ros::Subscriber sub_colli_;
84  ros::Subscriber sub_navgraph_;
85 
86  MoveBaseServer::GoalHandle as_goal_;
87  geometry_msgs::PoseStamped goal_pose_;
88 
89  std::string cfg_base_frame_;
90  std::string cfg_fixed_frame_;
91 
92  bool exec_as_;
93  bool exec_request_;
94  bool exec_running_;
95  ExecType exec_type_;
96  unsigned int exec_msgid_;
97 };
98 
99 #endif /* ROS_NAVIGATOR_THREAD_H__ */
Accept locomotion commands from ROS (emulate move_base).
RosMoveBaseThread()
Contructor.
virtual void loop()
Code to execute in the thread.
virtual void once()
Execute an action exactly once.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to use blocked timing.
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to log output.
Definition: logging.h:33
NavigatorInterface Fawkes BlackBoard Interface.
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
Thread class encapsulation of pthreads.
Definition: thread.h:46
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.