Fawkes API  Fawkes Development Version
navigator_thread.h
1 
2 /***************************************************************************
3  * navigator_thread.h - Robotino ROS Navigator Thread
4  *
5  * Created: Sat June 09 15:13:27 2012
6  * Copyright 2012 Sebastian Reuter
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_NAVIGATOR_THREAD_H_
23 #define _ROS_NAVIGATOR_THREAD_H_
24 
25 #include <actionlib/client/simple_action_client.h>
26 #include <aspect/blackboard.h>
27 #include <aspect/blocked_timing.h>
28 #include <aspect/clock.h>
29 #include <aspect/configurable.h>
30 #include <aspect/logging.h>
31 #include <aspect/tf.h>
32 #include <core/threading/thread.h>
33 #include <dynamic_reconfigure/Config.h>
34 #include <dynamic_reconfigure/DoubleParameter.h>
35 #include <dynamic_reconfigure/Reconfigure.h>
36 #include <interfaces/NavigatorInterface.h>
37 #include <move_base_msgs/MoveBaseAction.h>
38 #include <move_base_msgs/MoveBaseActionGoal.h>
39 #include <move_base_msgs/MoveBaseGoal.h>
40 #include <plugins/ros/aspect/ros.h>
41 #include <ros/ros.h>
42 #include <tf/types.h>
43 #include <utils/math/angle.h>
44 
45 #include <math.h>
46 
47 namespace fawkes {
48 class NavigatorInterface;
49 }
50 
52  public fawkes::ClockAspect,
54  public fawkes::LoggingAspect,
57  public fawkes::ROSAspect,
59 {
60 public:
61  RosNavigatorThread(std::string &cfg_prefix);
62 
63  virtual void init();
64  virtual void finalize();
65  virtual void loop();
66 
67  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
68 protected:
69  virtual void
70  run()
71  {
72  Thread::run();
73  }
74 
75 private:
76  void check_status();
77  void send_goal();
78  void stop_goals();
79  void load_config();
80  bool set_dynreconf_value(const std::string &path, const float value);
81 
82 private:
83  typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
84 
85  void activeCb();
86  void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback);
87  void doneCb(const actionlib::SimpleClientGoalState & state,
88  const move_base_msgs::MoveBaseResultConstPtr &result);
89 
90  void transform_to_fixed_frame();
91 
93  MoveBaseClient * ac_;
94  move_base_msgs::MoveBaseGoal goal_;
95  bool cmd_sent_;
96  bool connected_history_;
97 
98  fawkes::Time *ac_init_checktime_;
99 
100  std::string cfg_prefix_;
101 
102  // ROS dynamic reconfigure parts
103  dynamic_reconfigure::ReconfigureRequest dynreconf_srv_req;
104  dynamic_reconfigure::ReconfigureResponse dynreconf_srv_resp;
105  dynamic_reconfigure::DoubleParameter dynreconf_double_param;
106  dynamic_reconfigure::Config dynreconf_conf;
107 
108  std::string cfg_dynreconf_path_;
109  std::string cfg_dynreconf_trans_vel_name_;
110  std::string cfg_dynreconf_rot_vel_name_;
111 
112  std::string cfg_fixed_frame_;
113  float cfg_ori_tolerance_;
114  float cfg_trans_tolerance_;
115 
116  float param_max_vel;
117  float param_max_rot;
118 
119  geometry_msgs::PoseStamped base_position;
120  float goal_position_x;
121  float goal_position_y;
122  float goal_position_yaw;
123 
124  float goal_tolerance_trans;
125  float goal_tolerance_yaw;
126 };
127 
128 #endif /* ROS_NAVIGATOR_THREAD_H__ */
RosNavigatorThread::loop
virtual void loop()
Code to execute in the thread.
Definition: navigator_thread.cpp:216
RosNavigatorThread::init
virtual void init()
Initialize the thread.
Definition: navigator_thread.cpp:42
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
RosNavigatorThread
Send Fawkes locomotion commands off to ROS.
Definition: navigator_thread.h:59
fawkes::BlackBoardAspect
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
fawkes
Fawkes library namespace.
fawkes::LoggingAspect
Thread aspect to log output.
Definition: logging.h:33
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
RosNavigatorThread::RosNavigatorThread
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
Definition: navigator_thread.cpp:34
fawkes::ROSAspect
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::ConfigurableAspect
Thread aspect to access configuration data.
Definition: configurable.h:33
RosNavigatorThread::finalize
virtual void finalize()
Finalize the thread.
Definition: navigator_thread.cpp:68
fawkes::NavigatorInterface
NavigatorInterface Fawkes BlackBoard Interface.
Definition: NavigatorInterface.h:34
fawkes::ClockAspect
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
RosNavigatorThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: navigator_thread.h:70