void joyCB(const sensor_msgs::JoyConstPtr& joyMsg)
{
useHomog = false;
useMocap = false;
if (joyMsg->buttons[1]) // b - reset
{
resetPub.publish(std_msgs::Empty());
}
else if (joyMsg->buttons[0]) // a - land
{
landPub.publish(std_msgs::Empty());
}
else if (joyMsg->buttons[3]) // y - takeoff
{
takeoffPub.publish(std_msgs::Empty());
}
else if (!wallOverride)
{
if (joyMsg->buttons[4]) // LB - use homog
{
useHomog = true;
}
else if (joyMsg->buttons[5]) // RB - use mocap
{
useMocap = true;
}
else
{
desLinVel = tf::Vector3(-1*joy_deadband(joyMsg->axes[4]), joy_deadband(-1*joyMsg->axes[3]), joy_deadband(joyMsg->axes[1]));
desAngVel = tf::Vector3(0,0, joy_deadband(joyMsg->axes[0]));
}
}
}
void joyCB(const sensor_msgs::JoyConstPtr& joyMsg)
{
autonomy = false;
if (joyMsg->buttons[1]) // b - reset
{
resetPub.publish(std_msgs::Empty());
}
else if (joyMsg->buttons[0]) // a - land
{
landPub.publish(std_msgs::Empty());
}
else if (joyMsg->buttons[3]) // y - takeoff
{
takeoffPub.publish(std_msgs::Empty());
}
else
{
joyLinVel = tf::Vector3(joy_deadband(joyMsg->axes[4]), joy_deadband(joyMsg->axes[3]), joy_deadband(joyMsg->axes[1]));
joyAngVel = tf::Vector3(0,0, joy_deadband(joyMsg->axes[0]));
// autonomy
if (joyMsg->buttons[4]) // LB - UGV0
{
target = "ugv0";
autonomy = true;
}
else if (joyMsg->buttons[5]) // RB - UGV0
{
target = "ugv1";
autonomy = true;
}
}
}
/********** callback for the controller **********/
void joy_callback(const sensor_msgs::Joy& msg)
{
watchdogTimer.stop();
command_from_xbox = geometry_msgs::Twist();// cleaning the xbox twist message
a_button = msg.buttons[0];// a button lands
if (a_button > 0)
{
land_pub.publish(std_msgs::Empty());
}
x_button = msg.buttons[2];// x button sends constant
if (x_button > 0)
{
send_0 = true;
}
b_button = msg.buttons[1];// b button kills motors
if (b_button > 0)
{
reset_pub.publish(std_msgs::Empty());
}
y_button = msg.buttons[3];// y button takes off
if (y_button > 0)
{
takeoff_pub.publish(std_msgs::Empty());
}
dpad_l = msg.buttons[11];//dpad left value, fine adjustment of tilt angle negatively
dpad_r = msg.buttons[12];//dpad right value, fine adjustment of tilt angle positively
dpad_u = msg.buttons[13];//dpad up value, coarse adjustment of tilt angle positively
dpad_d = msg.buttons[14];//dpad down value, coarse adjustment of tilt angle negatively
int tilt_neg = -1*dpad_l - 10*dpad_d;// tilt negatively
int tilt_pos = 1*dpad_r + 10*dpad_u;// tilt positively
gimbal_state_desired.angular.y = gimbal_state_desired.angular.y + tilt_neg + tilt_pos;// adjust the gimbal
camera_state_pub.publish(gimbal_state_desired);// update the angle
left_bumper = msg.buttons[4];// left bumper for using the tracking as the controller output
right_bumper = msg.buttons[5];// right bumper for using the mocap as the controller output
start_autonomous = (left_bumper > 0) || (right_bumper > 0);// start the autonomous if either are greater than 0
right_stick_vert = joy_deadband(msg.axes[4]);// right thumbstick vert controls linear x
command_from_xbox.linear.x = joy_gain*right_stick_vert;
right_stick_horz = joy_deadband(msg.axes[3]);// right thumbstick horz controls linear y
command_from_xbox.linear.y = joy_gain*right_stick_horz;
left_stick_vert = joy_deadband(msg.axes[1]);// left thumbstick vert controls linear z
command_from_xbox.linear.z = joy_gain*left_stick_vert;
left_stick_horz = joy_deadband(msg.axes[0]);// left thumbstick horz controls angular z
command_from_xbox.angular.z = -1*joy_gain*left_stick_horz;
if (!start_autonomous)// if not using the autonomous velocities as output will indicate recieved a new control input
{
if (send_0)
{
//command_from_xbox.linear.x = 0;
//command_from_xbox.linear.y = 0;
//command_from_xbox.linear.z = 0;
//command_from_xbox.angular.z = 0;
send_0 = false;
}
recieved_command_from_xbox = true;
}
watchdogTimer.start();
}
