public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//twist
twist = new Twist();
twist.Randomize();
//covariance
if (covariance == null)
{
covariance = new double[36];
}
else
{
Array.Resize(ref covariance, 36);
}
for (int i = 0; i < covariance.Length; i++)
{
//covariance[i]
covariance[i] = (rand.Next() + rand.NextDouble());
}
}
public override byte[] Serialize(bool partofsomethingelse)
{
int currentIndex = 0, length = 0;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
List <byte[]> pieces = new List <byte[]>();
GCHandle h;
//header
if (header == null)
{
header = new Header();
}
pieces.Add(header.Serialize(true));
//twist
if (twist == null)
{
twist = new Twist();
}
pieces.Add(twist.Serialize(true));
//combine every array in pieces into one array and return it
int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
int __a_b__e = 0;
byte[] __a_b__d = new byte[__a_b__f];
foreach (var __p__ in pieces)
{
Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
__a_b__e += __p__.Length;
}
return(__a_b__d);
}
public void pubCmdMsg(double linear, double angular, double speed)
{
Messages.geometry_msgs.Twist msg;
msg = new Messages.geometry_msgs.Twist();
msg.linear = new Messages.geometry_msgs.Vector3();
msg.linear.x = linear * speed > 0?(block_up?0:linear * speed):(block_down?0:linear * speed);
msg.angular = new Messages.geometry_msgs.Vector3();
msg.angular.z = angular * speed;
pub.publish(msg);
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//header
header = new Header(SERIALIZEDSTUFF, ref currentIndex);
//twist
twist = new Twist(SERIALIZEDSTUFF, ref currentIndex);
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//header
header = new Header();
header.Randomize();
//twist
twist = new Twist();
twist.Randomize();
}
public override byte[] Serialize(bool partofsomethingelse)
{
int currentIndex = 0, length = 0;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
List <byte[]> pieces = new List <byte[]>();
GCHandle h;
//twist
if (twist == null)
{
twist = new Twist();
}
pieces.Add(twist.Serialize(true));
//covariance
hasmetacomponents |= false;
if (covariance == null)
{
covariance = new double[0];
}
for (int i = 0; i < covariance.Length; i++)
{
//covariance[i]
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(covariance[i], h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
}
//combine every array in pieces into one array and return it
int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
int __a_b__e = 0;
byte[] __a_b__d = new byte[__a_b__f];
foreach (var __p__ in pieces)
{
Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
__a_b__e += __p__.Length;
}
return(__a_b__d);
}
void TwistCallback(Twist msg)
{
if (inputCtrl.localInput)
{
return;
}
Vector3 linear = msg.linear.ToUnityVector();
Vector3 angular = msg.angular.ToUnityVector();
if (droneController != null)
{
if (!droneController.MotorsEnabled)
{
droneController.MotorsEnabled = true;
}
droneController.SetLinearVelocity(linear, true);
droneController.SetAngularVelocity(angular, true);
}
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//twist
twist = new Twist(SERIALIZEDSTUFF, ref currentIndex);
//covariance
hasmetacomponents |= false;
if (covariance == null)
{
covariance = new double[36];
}
else
{
Array.Resize(ref covariance, 36);
}
for (int i = 0; i < covariance.Length; i++)
{
//covariance[i]
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (SERIALIZEDSTUFF.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(SERIALIZEDSTUFF, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Alloc failed");
}
covariance[i] = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
}
}
// controller link dispatcher
public void Link(object sender, EventArgs dontcare)
{
// get state of player one
currentState = GamePad.GetState(PlayerIndex.One);
// if controller is connected...
if (currentState.IsConnected)
{
// ...say its connected in the textbloxk, color it green cuz its good to go
LinkTextBlock.Text = "Controller: Connected";
LinkTextBlock.Foreground = Brushes.Green;
foreach (Buttons b in Enum.GetValues(typeof(Buttons)))
{
Button(b);
}
double left_y = currentState.ThumbSticks.Left.Y;
double left_x = -currentState.ThumbSticks.Left.X;
if (_adr)
{
//left_x *= -1;
left_y *= -1;
}
gm.Twist vel = new gm.Twist { linear = new gm.Vector3 { x = left_y * _trans.Value }, angular = new gm.Vector3 { z = left_x * _rot.Value } };
if(velPub != null)
velPub.publish(vel);
//arm controls via joystick are done here.
double right_y = currentState.ThumbSticks.Right.Y;
// this is inverted to reflect mikes arm driver. right requires a negative number, not the default positive value
double right_x = -1 * currentState.ThumbSticks.Right.X;
double right_trigger = currentState.Triggers.Right;
if (!engaged && (Math.Abs(right_y) > .1 || Math.Abs(right_x) > .1 ))
{
engaged = true;
ArmON.publish(new m.Bool() { data = true });
Arm_Engaged.Content = "Arm Engaged";
Arm_Engaged.Background = Brushes.White;
Arm_Engaged.Foreground = Brushes.Green;
}
//if trigger is not pressed, send close signal ( -1 ). Th goal is to have the gripper
// going to a close state when the right trigger is not being pressed.
if (right_trigger == 0)
right_trigger = -1;
/*Console.WriteLine( "joy_right_x: " + right_x.ToString());
Console.WriteLine( "joy_right_y: " + right_y.ToString());
Console.WriteLine( "right trigger: " + right_trigger.ToString());*/
am.ArmMovement armmove = new am.ArmMovement();
armmove.pan_motor_velocity = right_x;
armmove.tilt_motor_velocity = right_y;
armmove.gripper_open = (right_trigger >= 0.5);
if (armPub != null)
armPub.publish(armmove);
}
// unless if controller is not connected...
else if (!currentState.IsConnected)
{
// ...have program complain controller is disconnected
LinkTextBlock.Text = "Controller: Disconnected";
LinkTextBlock.Foreground = Brushes.Red;
}
}
// controller link dispatcher
public void Link(object sender, EventArgs dontcare)
{
// get state of player one
currentState = GamePad.GetState(PlayerIndex.One);
// if controller is connected...
if (currentState.IsConnected)
{
// ...say its connected in the textbloxk, color it green cuz its good to go
LinkTextBlock.Text = "Controller: Connected";
LinkTextBlock.Foreground = Brushes.Green;
foreach (Buttons b in Enum.GetValues(typeof(Buttons)))
{
Button(b);
}
double left_y = currentState.ThumbSticks.Left.Y;
double left_x = -currentState.ThumbSticks.Left.X;
if (_adr)
{
//left_x *= -1;
left_y *= -1;
}
gm.Twist vel = new gm.Twist {
linear = new gm.Vector3 {
x = left_y * _trans.Value
}, angular = new gm.Vector3 {
z = left_x * _rot.Value
}
};
if (velPub != null)
{
velPub.publish(vel);
}
//arm controls via joystick are done here.
double right_y = currentState.ThumbSticks.Right.Y;
// this is inverted to reflect mikes arm driver. right requires a negative number, not the default positive value
double right_x = -1 * currentState.ThumbSticks.Right.X;
double right_trigger = currentState.Triggers.Right;
if (!engaged && (Math.Abs(right_y) > .1 || Math.Abs(right_x) > .1))
{
engaged = true;
ArmON.publish(new m.Bool()
{
data = true
});
Arm_Engaged.Content = "Arm Engaged";
Arm_Engaged.Background = Brushes.White;
Arm_Engaged.Foreground = Brushes.Green;
}
//if trigger is not pressed, send close signal ( -1 ). Th goal is to have the gripper
// going to a close state when the right trigger is not being pressed.
if (right_trigger == 0)
{
right_trigger = -1;
}
/*Console.WriteLine( "joy_right_x: " + right_x.ToString());
* Console.WriteLine( "joy_right_y: " + right_y.ToString());
* Console.WriteLine( "right trigger: " + right_trigger.ToString());*/
am.ArmMovement armmove = new am.ArmMovement();
armmove.pan_motor_velocity = right_x;
armmove.tilt_motor_velocity = right_y;
armmove.gripper_open = (right_trigger >= 0.5);
if (armPub != null)
{
armPub.publish(armmove);
}
}
// unless if controller is not connected...
else if (!currentState.IsConnected)
{
// ...have program complain controller is disconnected
LinkTextBlock.Text = "Controller: Disconnected";
LinkTextBlock.Foreground = Brushes.Red;
}
}
