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
47namespace fawkes {
48class NavigatorInterface;
49}
50
57 public fawkes::ROSAspect,
59{
60public:
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() */
68protected:
69 virtual void
71 {
72 Thread::run();
73 }
74
75private:
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
82private:
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__ */
Send Fawkes locomotion commands off to ROS.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void finalize()
Finalize the thread.
RosNavigatorThread(std::string &cfg_prefix)
Constructor.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
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
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Fawkes library namespace.