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> 62 geometry_msgs::PoseStamped
pose_;
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);
116 private_nh.
param<
double>(
"yaw_velocity_max",
axes_.yaw.factor, 90.0);
118 private_nh.
param<
int>(
"slow_button",
buttons_.slow.button, 4);
120 private_nh.
param<
int>(
"stop_button",
buttons_.stop.button, 2);
121 private_nh.
param<
int>(
"interrupt_button",
buttons_.interrupt.button, 3);
125 std::string control_mode;
126 private_nh.
param<std::string>(
"control_mode", control_mode,
"twist");
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");
135 if (control_mode ==
"attitude")
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);
142 joy_subscriber_ = node_handle_.
subscribe<sensor_msgs::Joy>(
"joy", 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",
151 else if (control_mode ==
"velocity")
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);
157 joy_subscriber_ = node_handle_.
subscribe<sensor_msgs::Joy>(
"joy", 1,
159 velocity_publisher_ = robot_nh.advertise<geometry_msgs::TwistStamped>(
"command/twist",
162 else if (control_mode ==
"position")
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);
168 joy_subscriber_ = node_handle_.
subscribe<sensor_msgs::Joy>(
"joy", 1,
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;
184 motor_enable_service_ = robot_nh.serviceClient<hector_uav_msgs::EnableMotors>(
200 hector_uav_msgs::AttitudeCommand attitude;
201 hector_uav_msgs::ThrustCommand
thrust;
202 hector_uav_msgs::YawrateCommand yawrate;
204 attitude.header.stamp = thrust.header.stamp = yawrate.header.stamp =
ros::Time::now();
215 attitude_publisher_.
publish(attitude);
218 thrust_publisher_.
publish(thrust);
220 yawrate.turnrate =
getAxis(joy,
axes_.yaw) * M_PI/180.0;
226 yawrate_publisher_.
publish(yawrate);
240 geometry_msgs::TwistStamped velocity;
247 velocity.twist.angular.z =
getAxis(joy,
axes_.yaw) * M_PI/180.0;
255 velocity_publisher_.
publish(velocity);
271 if (!pose_.header.stamp.isZero()) {
272 dt = std::max(0.0, std::min(1.0, (now - pose_.header.stamp).toSec()));
277 pose_.header.stamp = now;
288 hector_uav_msgs::PoseGoal goal;
289 goal.target_pose =
pose_;
290 pose_client_->sendGoal(goal);
304 if (axis.
axis == 0 || std::abs(axis.
axis) > joy->axes.size())
306 ROS_ERROR_STREAM(
"Axis " << axis.
axis <<
" out of range, joy has " << joy->axes.size() <<
" axes");
323 if (button.
button <= 0 || button.
button > joy->buttons.size())
325 ROS_ERROR_STREAM(
"Button " << button.
button <<
" out of range, joy has " << joy->buttons.size() <<
" buttons");
329 return joy->buttons[button.
button - 1] > 0;
336 ROS_WARN(
"Motor enable service not found");
340 hector_uav_msgs::EnableMotors srv;
341 srv.request.enable = enable;
342 return motor_enable_service_.
call(srv);
349 velocity_publisher_.
publish(geometry_msgs::TwistStamped());
353 attitude_publisher_.
publish(hector_uav_msgs::AttitudeCommand());
357 thrust_publisher_.
publish(hector_uav_msgs::ThrustCommand());
361 yawrate_publisher_.
publish(hector_uav_msgs::YawrateCommand());
368 int main(
int argc,
char **argv)
370 ros::init(argc, argv,
"quadrotor_teleop");
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)
geometry_msgs::PoseStamped pose_
actionlib::SimpleActionClient< hector_uav_msgs::PoseAction > PoseClient
std::string base_link_frame_
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))
std::string base_stabilized_frame_
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void joyPoseCallback(const sensor_msgs::JoyConstPtr &joy)
ros::NodeHandle node_handle_
actionlib::SimpleActionClient< hector_uav_msgs::TakeoffAction > TakeoffClient
boost::shared_ptr< LandingClient > landing_client_
int main(int argc, char **argv)
uint32_t getNumSubscribers() const
bool enableMotors(bool enable)
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_