controller.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 #include <std_srvs/Empty.h>
4 #include <geometry_msgs/Twist.h>
5 
6 
7 #include "pid.hpp"
8 
9 double get(
10  const ros::NodeHandle& n,
11  const std::string& name) {
12  double value;
13  n.getParam(name, value);
14  return value;
15 }
16 
18 {
19 public:
20 
22  const std::string& worldFrame,
23  const std::string& frame,
24  const ros::NodeHandle& n)
25  : m_worldFrame(worldFrame)
26  , m_frame(frame)
27  , m_pubNav()
28  , m_listener()
29  , m_pidX(
30  get(n, "PIDs/X/kp"),
31  get(n, "PIDs/X/kd"),
32  get(n, "PIDs/X/ki"),
33  get(n, "PIDs/X/minOutput"),
34  get(n, "PIDs/X/maxOutput"),
35  get(n, "PIDs/X/integratorMin"),
36  get(n, "PIDs/X/integratorMax"),
37  "x")
38  , m_pidY(
39  get(n, "PIDs/Y/kp"),
40  get(n, "PIDs/Y/kd"),
41  get(n, "PIDs/Y/ki"),
42  get(n, "PIDs/Y/minOutput"),
43  get(n, "PIDs/Y/maxOutput"),
44  get(n, "PIDs/Y/integratorMin"),
45  get(n, "PIDs/Y/integratorMax"),
46  "y")
47  , m_pidZ(
48  get(n, "PIDs/Z/kp"),
49  get(n, "PIDs/Z/kd"),
50  get(n, "PIDs/Z/ki"),
51  get(n, "PIDs/Z/minOutput"),
52  get(n, "PIDs/Z/maxOutput"),
53  get(n, "PIDs/Z/integratorMin"),
54  get(n, "PIDs/Z/integratorMax"),
55  "z")
56  , m_pidYaw(
57  get(n, "PIDs/Yaw/kp"),
58  get(n, "PIDs/Yaw/kd"),
59  get(n, "PIDs/Yaw/ki"),
60  get(n, "PIDs/Yaw/minOutput"),
61  get(n, "PIDs/Yaw/maxOutput"),
62  get(n, "PIDs/Yaw/integratorMin"),
63  get(n, "PIDs/Yaw/integratorMax"),
64  "yaw")
65  , m_state(Idle)
66  , m_goal()
67  , m_subscribeGoal()
69  , m_serviceLand()
70  , m_thrust(0)
71  , m_startZ(0)
72  {
73  ros::NodeHandle nh;
75  m_pubNav = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
76  m_subscribeGoal = nh.subscribe("goal", 1, &Controller::goalChanged, this);
78  m_serviceLand = nh.advertiseService("land", &Controller::land, this);
79  }
80 
81  void run(double frequency)
82  {
83  ros::NodeHandle node;
84  ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Controller::iteration, this);
85  ros::spin();
86  }
87 
88 private:
90  const geometry_msgs::PoseStamped::ConstPtr& msg)
91  {
92  m_goal = *msg;
93  }
94 
95  bool takeoff(
96  std_srvs::Empty::Request& req,
97  std_srvs::Empty::Response& res)
98  {
99  ROS_INFO("Takeoff requested!");
100  m_state = TakingOff;
101 
102  tf::StampedTransform transform;
104  m_startZ = transform.getOrigin().z();
105 
106  return true;
107  }
108 
109  bool land(
110  std_srvs::Empty::Request& req,
111  std_srvs::Empty::Response& res)
112  {
113  ROS_INFO("Landing requested!");
114  m_state = Landing;
115 
116  return true;
117  }
118 
120  const std::string& sourceFrame,
121  const std::string& targetFrame,
122  tf::StampedTransform& result)
123  {
124  m_listener.lookupTransform(sourceFrame, targetFrame, ros::Time(0), result);
125  }
126 
127  void pidReset()
128  {
129  m_pidX.reset();
130  m_pidY.reset();
131  m_pidZ.reset();
132  m_pidYaw.reset();
133  }
134 
135  void iteration(const ros::TimerEvent& e)
136  {
137  float dt = e.current_real.toSec() - e.last_real.toSec();
138 
139  switch(m_state)
140  {
141  case TakingOff:
142  {
143  tf::StampedTransform transform;
145  if (transform.getOrigin().z() > m_startZ + 0.05 || m_thrust > 50000)
146  {
147  pidReset();
149  m_state = Automatic;
150  m_thrust = 0;
151  }
152  else
153  {
154  m_thrust += 10000 * dt;
155  geometry_msgs::Twist msg;
156  msg.linear.z = m_thrust;
157  m_pubNav.publish(msg);
158  }
159 
160  }
161  break;
162  case Landing:
163  {
164  m_goal.pose.position.z = m_startZ + 0.05;
165  tf::StampedTransform transform;
167  if (transform.getOrigin().z() <= m_startZ + 0.05) {
168  m_state = Idle;
169  geometry_msgs::Twist msg;
170  m_pubNav.publish(msg);
171  }
172  }
173  // intentional fall-thru
174  case Automatic:
175  {
176  tf::StampedTransform transform;
178 
179  geometry_msgs::PoseStamped targetWorld;
180  targetWorld.header.stamp = transform.stamp_;
181  targetWorld.header.frame_id = m_worldFrame;
182  targetWorld.pose = m_goal.pose;
183 
184  geometry_msgs::PoseStamped targetDrone;
185  m_listener.transformPose(m_frame, targetWorld, targetDrone);
186 
187  tfScalar roll, pitch, yaw;
190  targetDrone.pose.orientation.x,
191  targetDrone.pose.orientation.y,
192  targetDrone.pose.orientation.z,
193  targetDrone.pose.orientation.w
194  )).getRPY(roll, pitch, yaw);
195 
196  geometry_msgs::Twist msg;
197  msg.linear.x = m_pidX.update(0, targetDrone.pose.position.x);
198  msg.linear.y = m_pidY.update(0.0, targetDrone.pose.position.y);
199  msg.linear.z = m_pidZ.update(0.0, targetDrone.pose.position.z);
200  msg.angular.z = m_pidYaw.update(0.0, yaw);
201  m_pubNav.publish(msg);
202 
203 
204  }
205  break;
206  case Idle:
207  {
208  geometry_msgs::Twist msg;
209  m_pubNav.publish(msg);
210  }
211  break;
212  }
213  }
214 
215 private:
216 
217  enum State
218  {
219  Idle = 0,
222  Landing = 3,
223  };
224 
225 private:
226  std::string m_worldFrame;
227  std::string m_frame;
235  geometry_msgs::PoseStamped m_goal;
239  float m_thrust;
240  float m_startZ;
241 };
242 
243 int main(int argc, char **argv)
244 {
245  ros::init(argc, argv, "controller");
246 
247  // Read parameters
248  ros::NodeHandle n("~");
249  std::string worldFrame;
250  n.param<std::string>("worldFrame", worldFrame, "/world");
251  std::string frame;
252  n.getParam("frame", frame);
253  double frequency;
254  n.param("frequency", frequency, 50.0);
255 
256  Controller controller(worldFrame, frame, n);
257  controller.run(frequency);
258 
259  return 0;
260 }
void getTransform(const std::string &sourceFrame, const std::string &targetFrame, tf::StampedTransform &result)
Definition: controller.cpp:119
float m_startZ
Definition: controller.cpp:240
void iteration(const ros::TimerEvent &e)
Definition: controller.cpp:135
bool takeoff(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: controller.cpp:95
void publish(const boost::shared_ptr< M > &message) const
double tfScalar
float update(float value, float targetValue)
Definition: pid.hpp:47
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::ServiceServer m_serviceLand
Definition: controller.cpp:238
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
bool land(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
Definition: controller.cpp:109
geometry_msgs::PoseStamped m_goal
Definition: controller.cpp:235
ros::ServiceServer m_serviceTakeoff
Definition: controller.cpp:237
ros::Publisher m_pubNav
Definition: controller.cpp:228
ROSCPP_DECL void spin(Spinner &spinner)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
TFSIMD_FORCE_INLINE const tfScalar & z() const
ros::Subscriber m_subscribeGoal
Definition: controller.cpp:236
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Definition: pid.hpp:5
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void reset()
Definition: pid.hpp:30
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)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
State m_state
Definition: controller.cpp:234
void pidReset()
Definition: controller.cpp:127
tf::TransformListener m_listener
Definition: controller.cpp:229
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
void goalChanged(const geometry_msgs::PoseStamped::ConstPtr &msg)
Definition: controller.cpp:89
bool getParam(const std::string &key, std::string &s) const
void run(double frequency)
Definition: controller.cpp:81
Controller(const std::string &worldFrame, const std::string &frame, const ros::NodeHandle &n)
Definition: controller.cpp:21
std::string m_worldFrame
Definition: controller.cpp:226
double get(const ros::NodeHandle &n, const std::string &name)
Definition: controller.cpp:9
int main(int argc, char **argv)
Definition: controller.cpp:243
float ki() const
Definition: pid.hpp:42
float m_thrust
Definition: controller.cpp:239
void setIntegral(float integral)
Definition: pid.hpp:37
std::string m_frame
Definition: controller.cpp:227


crazyflie_controller
Author(s): Wolfgang Hoenig
autogenerated on Tue May 19 2020 03:38:04