public YawRateCommand limit (YawRateCommand input)
{
YawRateCommand output = new YawRateCommand ();
output.header = input.header;
output.turnrate = (float) turnrate.limit ( input.turnrate );
return output;
}
void yawrateCommandCb (YawRateCommand command)
{
lock ( command_mutex_ )
{
yawrate_command_ = command;
if ( yawrate_command_.header.Stamp.data.toSec () == 0.0 )
{
yawrate_command_.header.Stamp = ROS.GetTime ();
}
}
}
public AttitudeSubscriberHelper (NodeHandle nh, object command_mutex, AttitudeCommand attitude_command, YawRateCommand yawrate_command, ThrustCommand thrust_command)
{
command_mutex_ = command_mutex;
attitude_command_ = attitude_command;
yawrate_command_ = yawrate_command;
thrust_command_ = thrust_command;
attitude_subscriber_ = nh.subscribe<AttitudeCommand> ( "attitude", 1, attitudeCommandCb );
yawrate_subscriber_ = nh.subscribe<YawRateCommand> ( "yawrate", 1, yawrateCommandCb );
thrust_subscriber_ = nh.subscribe<ThrustCommand> ( "thrust", 1, thrustCommandCb );
}
public void joyAttitudeCallback(Joy joy)
{
AttitudeCommand attitude = new AttitudeCommand();
ThrustCommand thrust = new ThrustCommand();
YawRateCommand yawrate = new YawRateCommand();
attitude.header.Stamp = thrust.header.Stamp = yawrate.header.Stamp = ROS.GetTime();
attitude.header.Frame_id = yawrate.header.Frame_id = baseStabilizedFrame;
thrust.header.Frame_id = baseLinkFrame;
attitude.roll = (float)(-getAxis(joy, sAxes.y) * Math.PI / 180.0);
attitude.pitch = (float)(getAxis(joy, sAxes.x) * Math.PI / 180.0);
if (getButton(joy, sButtons.slow))
{
attitude.roll *= (float)slowFactor;
attitude.pitch *= (float)slowFactor;
}
attitudePublisher.publish(attitude);
thrust.thrust = (float)getAxis(joy, sAxes.thrust);
thrustPublisher.publish(thrust);
yawrate.turnrate = (float)(getAxis(joy, sAxes.yaw) * Math.PI / 180.0);
if (getButton(joy, sButtons.slow))
{
yawrate.turnrate *= (float)slowFactor;
}
yawRatePublisher.publish(yawrate);
if (getButton(joy, sButtons.stop))
{
enableMotors(false);
}
else if (getButton(joy, sButtons.go))
{
enableMotors(true);
}
}
