quadrotor_teleop.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012-2016, Institute of Flight Systems and Automatic Control,
3 // Technische Universität Darmstadt.
4 // All rights reserved.
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of hector_quadrotor nor the names of its contributors
14 // may be used to endorse or promote products derived from this software
15 // without specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #include <ros/ros.h>
30 #include <sensor_msgs/Joy.h>
31 #include <geometry_msgs/TwistStamped.h>
32 #include <geometry_msgs/PoseStamped.h>
33 #include <hector_uav_msgs/YawrateCommand.h>
34 #include <hector_uav_msgs/ThrustCommand.h>
35 #include <hector_uav_msgs/AttitudeCommand.h>
36 #include <hector_uav_msgs/TakeoffAction.h>
37 #include <hector_uav_msgs/LandingAction.h>
38 #include <hector_uav_msgs/PoseAction.h>
41 #include <hector_uav_msgs/EnableMotors.h>
43 
45 {
46 
47 class Teleop
48 {
49 private:
53 
61 
62  geometry_msgs::PoseStamped pose_;
63  double yaw_;
64 
65  struct Axis
66  {
67  Axis()
68  : axis(0), factor(0.0), offset(0.0)
69  {}
70 
71  int axis;
72  double factor;
73  double offset;
74  };
75 
76  struct Button
77  {
79  : button(0)
80  {}
81 
82  int button;
83  };
84 
85  struct
86  {
92  } axes_;
93 
94  struct
95  {
100  } buttons_;
101 
102  double slow_factor_;
104 
105 public:
107  {
108  ros::NodeHandle private_nh("~");
109 
110  private_nh.param<int>("x_axis", axes_.x.axis, 5);
111  private_nh.param<int>("y_axis", axes_.y.axis, 4);
112  private_nh.param<int>("z_axis", axes_.z.axis, 2);
113  private_nh.param<int>("thrust_axis", axes_.thrust.axis, -3);
114  private_nh.param<int>("yaw_axis", axes_.yaw.axis, 1);
115 
116  private_nh.param<double>("yaw_velocity_max", axes_.yaw.factor, 90.0);
117 
118  private_nh.param<int>("slow_button", buttons_.slow.button, 4);
119  private_nh.param<int>("go_button", buttons_.go.button, 1);
120  private_nh.param<int>("stop_button", buttons_.stop.button, 2);
121  private_nh.param<int>("interrupt_button", buttons_.interrupt.button, 3);
122  private_nh.param<double>("slow_factor", slow_factor_, 0.2);
123 
124  // TODO dynamic reconfig
125  std::string control_mode;
126  private_nh.param<std::string>("control_mode", control_mode, "twist");
127 
128  ros::NodeHandle robot_nh;
129 
130  // TODO factor out
131  robot_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
132  robot_nh.param<std::string>("world_frame", world_frame_, "world");
133  robot_nh.param<std::string>("base_stabilized_frame", base_stabilized_frame_, "base_stabilized");
134 
135  if (control_mode == "attitude")
136  {
137  private_nh.param<double>("pitch_max", axes_.x.factor, 30.0);
138  private_nh.param<double>("roll_max", axes_.y.factor, 30.0);
139  private_nh.param<double>("thrust_max", axes_.thrust.factor, 10.0);
140  private_nh.param<double>("thrust_offset", axes_.thrust.offset, 10.0);
141 
142  joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1,
143  boost::bind(&Teleop::joyAttitudeCallback, this, _1));
144  attitude_publisher_ = robot_nh.advertise<hector_uav_msgs::AttitudeCommand>(
145  "command/attitude", 10);
146  yawrate_publisher_ = robot_nh.advertise<hector_uav_msgs::YawrateCommand>(
147  "command/yawrate", 10);
148  thrust_publisher_ = robot_nh.advertise<hector_uav_msgs::ThrustCommand>("command/thrust",
149  10);
150  }
151  else if (control_mode == "velocity")
152  {
153  private_nh.param<double>("x_velocity_max", axes_.x.factor, 2.0);
154  private_nh.param<double>("y_velocity_max", axes_.y.factor, 2.0);
155  private_nh.param<double>("z_velocity_max", axes_.z.factor, 2.0);
156 
157  joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1,
158  boost::bind(&Teleop::joyTwistCallback, this, _1));
159  velocity_publisher_ = robot_nh.advertise<geometry_msgs::TwistStamped>("command/twist",
160  10);
161  }
162  else if (control_mode == "position")
163  {
164  private_nh.param<double>("x_velocity_max", axes_.x.factor, 2.0);
165  private_nh.param<double>("y_velocity_max", axes_.y.factor, 2.0);
166  private_nh.param<double>("z_velocity_max", axes_.z.factor, 2.0);
167 
168  joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1,
169  boost::bind(&Teleop::joyPoseCallback, this, _1));
170 
171  pose_.pose.position.x = 0;
172  pose_.pose.position.y = 0;
173  pose_.pose.position.z = 0;
174  pose_.pose.orientation.x = 0;
175  pose_.pose.orientation.y = 0;
176  pose_.pose.orientation.z = 0;
177  pose_.pose.orientation.w = 1;
178  }
179  else
180  {
181  ROS_ERROR_STREAM("Unsupported control mode: " << control_mode);
182  }
183 
184  motor_enable_service_ = robot_nh.serviceClient<hector_uav_msgs::EnableMotors>(
185  "enable_motors");
186  takeoff_client_ = boost::shared_ptr<TakeoffClient>(new TakeoffClient(robot_nh, "action/takeoff"));
187  landing_client_ = boost::shared_ptr<LandingClient>(new LandingClient(robot_nh, "action/landing"));
188  pose_client_ = boost::shared_ptr<PoseClient>(new PoseClient(robot_nh, "action/pose"));
189 
190  }
191 
193  {
194  stop();
195  }
196 
197  void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
198  {
199 
200  hector_uav_msgs::AttitudeCommand attitude;
201  hector_uav_msgs::ThrustCommand thrust;
202  hector_uav_msgs::YawrateCommand yawrate;
203 
204  attitude.header.stamp = thrust.header.stamp = yawrate.header.stamp = ros::Time::now();
205  attitude.header.frame_id = yawrate.header.frame_id = base_stabilized_frame_;
206  thrust.header.frame_id = base_link_frame_;
207 
208  attitude.roll = -getAxis(joy, axes_.y) * M_PI/180.0;
209  attitude.pitch = getAxis(joy, axes_.x) * M_PI/180.0;
210  if (getButton(joy, buttons_.slow))
211  {
212  attitude.roll *= slow_factor_;
213  attitude.pitch *= slow_factor_;
214  }
215  attitude_publisher_.publish(attitude);
216 
217  thrust.thrust = getAxis(joy, axes_.thrust);
218  thrust_publisher_.publish(thrust);
219 
220  yawrate.turnrate = getAxis(joy, axes_.yaw) * M_PI/180.0;
221  if (getButton(joy, buttons_.slow))
222  {
223  yawrate.turnrate *= slow_factor_;
224  }
225 
226  yawrate_publisher_.publish(yawrate);
227 
228  if (getButton(joy, buttons_.stop))
229  {
230  enableMotors(false);
231  }
232  else if (getButton(joy, buttons_.go))
233  {
234  enableMotors(true);
235  }
236  }
237 
238  void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
239  {
240  geometry_msgs::TwistStamped velocity;
241  velocity.header.frame_id = base_stabilized_frame_;
242  velocity.header.stamp = ros::Time::now();
243 
244  velocity.twist.linear.x = getAxis(joy, axes_.x);
245  velocity.twist.linear.y = getAxis(joy, axes_.y);
246  velocity.twist.linear.z = getAxis(joy, axes_.z);
247  velocity.twist.angular.z = getAxis(joy, axes_.yaw) * M_PI/180.0;
248  if (getButton(joy, buttons_.slow))
249  {
250  velocity.twist.linear.x *= slow_factor_;
251  velocity.twist.linear.y *= slow_factor_;
252  velocity.twist.linear.z *= slow_factor_;
253  velocity.twist.angular.z *= slow_factor_;
254  }
255  velocity_publisher_.publish(velocity);
256 
257  if (getButton(joy, buttons_.stop))
258  {
259  enableMotors(false);
260  }
261  else if (getButton(joy, buttons_.go))
262  {
263  enableMotors(true);
264  }
265  }
266 
267  void joyPoseCallback(const sensor_msgs::JoyConstPtr &joy)
268  {
269  ros::Time now = ros::Time::now();
270  double dt = 0.0;
271  if (!pose_.header.stamp.isZero()) {
272  dt = std::max(0.0, std::min(1.0, (now - pose_.header.stamp).toSec()));
273  }
274 
275  if (getButton(joy, buttons_.go))
276  {
277  pose_.header.stamp = now;
278  pose_.header.frame_id = world_frame_;
279  pose_.pose.position.x += (cos(yaw_) * getAxis(joy, axes_.x) - sin(yaw_) * getAxis(joy, axes_.y)) * dt;
280  pose_.pose.position.y += (cos(yaw_) * getAxis(joy, axes_.y) + sin(yaw_) * getAxis(joy, axes_.x)) * dt;
281  pose_.pose.position.z += getAxis(joy, axes_.z) * dt;
282  yaw_ += getAxis(joy, axes_.yaw) * M_PI/180.0 * dt;
283 
284  tf2::Quaternion q;
285  q.setRPY(0.0, 0.0, yaw_);
286  pose_.pose.orientation = tf2::toMsg(q);
287 
288  hector_uav_msgs::PoseGoal goal;
289  goal.target_pose = pose_;
290  pose_client_->sendGoal(goal);
291  }
292  if (getButton(joy, buttons_.interrupt))
293  {
294  pose_client_->cancelGoalsAtAndBeforeTime(ros::Time::now());
295  }
296  if (getButton(joy, buttons_.stop))
297  {
298  landing_client_->sendGoalAndWait(hector_uav_msgs::LandingGoal(), ros::Duration(10.0), ros::Duration(10.0));
299  }
300  }
301 
302  double getAxis(const sensor_msgs::JoyConstPtr &joy, const Axis &axis)
303  {
304  if (axis.axis == 0 || std::abs(axis.axis) > joy->axes.size())
305  {
306  ROS_ERROR_STREAM("Axis " << axis.axis << " out of range, joy has " << joy->axes.size() << " axes");
307  return 0;
308  }
309 
310  double output = std::abs(axis.axis) / axis.axis * joy->axes[std::abs(axis.axis) - 1] * axis.factor + axis.offset;
311 
312  // TODO keep or remove deadzone? may not be needed
313  // if (std::abs(output) < axis.max_ * 0.2)
314  // {
315  // output = 0.0;
316  // }
317 
318  return output;
319  }
320 
321  bool getButton(const sensor_msgs::JoyConstPtr &joy, const Button &button)
322  {
323  if (button.button <= 0 || button.button > joy->buttons.size())
324  {
325  ROS_ERROR_STREAM("Button " << button.button << " out of range, joy has " << joy->buttons.size() << " buttons");
326  return false;
327  }
328 
329  return joy->buttons[button.button - 1] > 0;
330  }
331 
332  bool enableMotors(bool enable)
333  {
334  if (!motor_enable_service_.waitForExistence(ros::Duration(5.0)))
335  {
336  ROS_WARN("Motor enable service not found");
337  return false;
338  }
339 
340  hector_uav_msgs::EnableMotors srv;
341  srv.request.enable = enable;
342  return motor_enable_service_.call(srv);
343  }
344 
345  void stop()
346  {
347  if (velocity_publisher_.getNumSubscribers() > 0)
348  {
349  velocity_publisher_.publish(geometry_msgs::TwistStamped());
350  }
351  if (attitude_publisher_.getNumSubscribers() > 0)
352  {
353  attitude_publisher_.publish(hector_uav_msgs::AttitudeCommand());
354  }
355  if (thrust_publisher_.getNumSubscribers() > 0)
356  {
357  thrust_publisher_.publish(hector_uav_msgs::ThrustCommand());
358  }
359  if (yawrate_publisher_.getNumSubscribers() > 0)
360  {
361  yawrate_publisher_.publish(hector_uav_msgs::YawrateCommand());
362  }
363  }
364 };
365 
366 } // namespace hector_quadrotor
367 
368 int main(int argc, char **argv)
369 {
370  ros::init(argc, argv, "quadrotor_teleop");
371 
373  ros::spin();
374 
375  return 0;
376 }
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
void publish(const boost::shared_ptr< M > &message) const
bool getButton(const sensor_msgs::JoyConstPtr &joy, const Button &button)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
actionlib::SimpleActionClient< hector_uav_msgs::LandingAction > LandingClient
ros::Publisher velocity_publisher_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
boost::shared_ptr< PoseClient > pose_client_
void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
#define ROS_WARN(...)
geometry_msgs::PoseStamped pose_
actionlib::SimpleActionClient< hector_uav_msgs::PoseAction > PoseClient
ROSCPP_DECL void spin(Spinner &spinner)
ros::Publisher attitude_publisher_
ros::ServiceClient motor_enable_service_
struct hector_quadrotor::Teleop::@0 axes_
ros::Subscriber joy_subscriber_
ros::Publisher thrust_publisher_
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool param(const std::string &param_name, T &param_val, const T &default_val) const
void joyPoseCallback(const sensor_msgs::JoyConstPtr &joy)
actionlib::SimpleActionClient< hector_uav_msgs::TakeoffAction > TakeoffClient
boost::shared_ptr< LandingClient > landing_client_
B toMsg(const A &a)
int main(int argc, char **argv)
uint32_t getNumSubscribers() const
bool enableMotors(bool enable)
static Time now()
struct hector_quadrotor::Teleop::@1 buttons_
boost::shared_ptr< TakeoffClient > takeoff_client_
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
double getAxis(const sensor_msgs::JoyConstPtr &joy, const Axis &axis)
#define ROS_ERROR_STREAM(args)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
ros::Publisher yawrate_publisher_


hector_quadrotor_teleop
Author(s): Johannes Meyer
autogenerated on Mon Jun 10 2019 13:37:04