We Keep Coding

Uploading waypoints, spline waypoints to APM Quadcopter

I'm building my mini GCS. I had Arm/Disarm are working correctly, but I'm stuck in how to send the waypoints to my drone. I need to send a Spline waypoints. I've tried MAV_CMD_NAV_SPLINE_WAYPOINT and set the parameters as required, but it does not send anything to my drone .
Here is my code:
MAVLink.mavlink_command_long_t alaa = new MAVLink.mavlink_command_long_t();
alaa.target_system = 1;
alaa.target_component = 1;
alaa.command = (ushort)MAV_CMD.MAV_CMD_NAV_SPLINE_WAYPOINT;
alaa.param1 = 0; // hold time
alaa.param4 = 0; // yaw angle
alaa.param5 = latt; // desired latitude
alaa.param6 = longg; // desired longitude
alaa.param7 = altt; // desired altitude
byte[] packet = mavAlaaWrite.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, alaa);
Serial.Write(packet, 0, packet.Length);
Can someone please make a simple example by sending mission spline waypoints to my drone by clicking on a command button?
I'm programming using C# Windows forms.
Many thanks in advance.

Store Kinect's v2.0 Motion to BVH File

I would like to store the motion capture data from Kinect 2 as a BVH file. I found code which does so for Kinect 1 which can be found here. I went through the code and found several things that I was not able to understand.
For example, in the mentioned code I've tried to understand what exactly the Skeleton skel object, found in several places in the code, actually is. If not, are there any known application available to accomplish the intended?
EDIT: I tried to change Skeleton skel to Body skel which I think is the correspondant object for kinect SDK 2.0. However I've got an error when I try to get the position of the body:
tempMotionVektor[0] = -Math.Round( skel.Position.X * 100,2);
tempMotionVektor[1] = Math.Round( skel.Position.Y * 100,2) + 120;
tempMotionVektor[2] = 300 - Math.Round( skel.Position.Z * 100,2);
I've gotten errors when calling the function Position for the Body skel. How can I retrieve the X, Y, Z of the skeleton in sdk 2.0?? I tried to change the above three lines to:
tempMotionVektor[0] = -Math.Round(skel.Joints[0].Position.X * 100, 2);
tempMotionVektor[1] = Math.Round(skel.Joints[0].Position.Y * 100, 2) + 120;
tempMotionVektor[2] = 300 - Math.Round(skel.Joints[0].Position.Z * 100, 2);
EDIT: Basically I managed to store the a bvh file after combining bodyBasicsWPF and kinect2bvh. However, it seems that the skeleton I am storing is not efficient. There are strange movements in the elbows. I am trying to understand if I have to change something in the file kinectSkeletonBVH.cp. More specifically, what are the changes in the joint axis orientation for the kinect 2 version. How can I change the following line: skel.BoneOrientations[JointType.ShoulderCenter].AbsoluteRotation.Quaternion; I tried to change that line with skel.JointOrientations[JointType.ShoulderCenter].Orientation. Am I right? I am using the following code to add the joint to BVHBone objects:
BVHBone hipCenter = new BVHBone(null, JointType.SpineBase.ToString(), 6, TransAxis.None, true);
BVHBone hipCenter2 = new BVHBone(hipCenter, "HipCenter2", 3, TransAxis.Y, false);
BVHBone spine = new BVHBone(hipCenter2, JointType.SpineMid.ToString(), 3, TransAxis.Y, true);
BVHBone shoulderCenter = new BVHBone(spine, JointType.SpineShoulder.ToString(), 3, TransAxis.Y, true);
BVHBone collarLeft = new BVHBone(shoulderCenter, "CollarLeft", 3, TransAxis.X, false);
BVHBone shoulderLeft = new BVHBone(collarLeft, JointType.ShoulderLeft.ToString(), 3, TransAxis.X, true);
BVHBone elbowLeft = new BVHBone(shoulderLeft, JointType.ElbowLeft.ToString(), 3, TransAxis.X, true);
BVHBone wristLeft = new BVHBone(elbowLeft, JointType.WristLeft.ToString(), 3, TransAxis.X, true);
BVHBone handLeft = new BVHBone(wristLeft, JointType.HandLeft.ToString(), 0, TransAxis.X, true);
BVHBone neck = new BVHBone(shoulderCenter, "Neck", 3, TransAxis.Y, false);
BVHBone head = new BVHBone(neck, JointType.Head.ToString(), 3, TransAxis.Y, true);
BVHBone headtop = new BVHBone(head, "Headtop", 0, TransAxis.None, false);
I can't understand where inside the code the axis for every Joint is calculated.

The code you used for Kinect 1.0 to obtain a BVH file use the joints information to build bone vectors by reading the Skeleton.
public static double[] getBoneVectorOutofJointPosition(BVHBone bvhBone, Skeleton skel)
{
double[] boneVector = new double[3] { 0, 0, 0 };
double[] boneVectorParent = new double[3] { 0, 0, 0 };
string boneName = bvhBone.Name;
JointType Joint;
if (bvhBone.Root == true)
{
boneVector = new double[3] { 0, 0, 0 };
}
else
{
if (bvhBone.IsKinectJoint == true)
{
Joint = KinectSkeletonBVH.String2JointType(boneName);
boneVector[0] = skel.Joints[Joint].Position.X;
boneVector[1] = skel.Joints[Joint].Position.Y;
boneVector[2] = skel.Joints[Joint].Position.Z;
..
Source: Nguyên Lê Đặng - Kinect2BVH.V2
Except in Kinect 2.0, Skeleton class has been replaced by the Body class, so you need to change it to deal with a Body instead, and obtain the joints by following the steps quoted below.
// Kinect namespace
using Microsoft.Kinect;
// ...
// Kinect sensor and Kinect stream reader objects
KinectSensor _sensor;
MultiSourceFrameReader _reader;
IList<Body> _bodies;
// Kinect sensor initialization
_sensor = KinectSensor.GetDefault();
if (_sensor != null)
{
_sensor.Open();
}
We also added a list of bodies, where all of the body/skeleton related
data will be saved. If you have developed for Kinect version 1, you
notice that the Skeleton class has been replaced by the Body class.
Remember the MultiSourceFrameReader? This class gives us access on
every stream, including the body stream! We simply need to let the
sensor know that we need body tracking functionality by adding an
additional parameter when initializing the reader:
_reader = _sensor.OpenMultiSourceFrameReader(FrameSourceTypes.Color |
FrameSourceTypes.Depth |
FrameSourceTypes.Infrared |
FrameSourceTypes.Body);
_reader.MultiSourceFrameArrived += Reader_MultiSourceFrameArrived;
The Reader_MultiSourceFrameArrived method will be called whenever a
new frame is available. Let’s specify what will happen in terms of the
body data:
Get a reference to the body frame
Check whether the body frame is null – this is crucial
Initialize the _bodies list
Call the GetAndRefreshBodyData method, so as to copy the body data into the list
Loop through the list of bodies and do awesome stuff!
Always remember to check for null values. Kinect provides you with
approximately 30 frames per second – anything could be null or
missing! Here is the code so far:
void Reader_MultiSourceFrameArrived(object sender,
MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference.AcquireFrame();
// Color
// ...
// Depth
// ...
// Infrared
// ...
// Body
using (var frame = reference.BodyFrameReference.AcquireFrame())
{
if (frame != null)
{
_bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(_bodies);
foreach (var body in _bodies)
{
if (body != null)
{
// Do something with the body...
}
}
}
}
}
This is it! We now have access to the bodies Kinect identifies. Next
step is to display the skeleton information on-screen. Each body
consists of 25 joints. The sensor provides us with the position (X, Y,
Z) and the rotation information for each one of them. Moreover, Kinect
lets us know whether the joints are tracked, hypothsized or not
tracked. It’s a good practice to check whether a body is tracked
before performing any critical functions.
The following code illustrates how we can access the different body
joints:
if (body != null)
{
if (body.IsTracked)
{
Joint head = body.Joints[JointType.Head];
float x = head.Position.X;
float y = head.Position.Y;
float z = head.Position.Z;
// Draw the joints...
}
}
Source: Vangos Pterneas Blog - KINECT FOR WINDOWS VERSION 2: BODY TRACKING

Removing features from the Google Earth Plugin in c#

Using the Visual Studio C# Winforms Google Earth plugin, 4 placemarks have been added to the globe as can be seen in the picture below:
My goal is to be able to remove the linestring placemark. The steps would seem to be to get all the placemarks, find the linestring and remove it.
Here is the code being used to create the linestring placemarks (more or less from the API website)
var lineStringPlacemark = ge2.createPlacemark("Line_" + name);
// create the line string geometry
var lineString = ge2.createLineString("");
lineStringPlacemark.setGeometry(lineString);
// add the the points to the line string geometry
double dlat1 = Convert.ToDouble(lat1) / 100000;
double dlon1 = Convert.ToDouble(lon1) / 100000;
double dlat2 = Convert.ToDouble(lat2) / 100000;
double dlon2 = Convert.ToDouble(lon2) / 100000;
lineString.getCoordinates().pushLatLngAlt(dlat1, dlon1, 0);
lineString.getCoordinates().pushLatLngAlt(dlat2, dlon2, 0);
// Create a style and set width and color of line
lineStringPlacemark.setStyleSelector(ge2.createStyle(""));
var lineStyle = lineStringPlacemark.getStyleSelector().getLineStyle();
lineStyle.setWidth(5);
lineStyle.getColor().set("9900ffff"); // aabbggrr format
// Add the feature to Earth
ge2.getFeatures().appendChild(lineStringPlacemark);
And here is the code I ended up using to remove the line. Note that the GEHelpers.RemoveFeatureById(ge2, s); is commented out since it isn't working for me for some reason.
for (int i = 0; i < ge2.getFeatures().getChildNodes().getLength(); i++)
{
var kmlobject = ge2.getFeatures().getChildNodes().item[i];
string s = kmlobject.getId();
if (s.Contains("Line_"))
{
ge2.getFeatures().removeChild(kmlobject);
kmlobject.release();
//GEHelpers.RemoveFeatureById(ge2, s);
}
}

The line you have should work and remove all the currently loaded content.
GEHelpers.RemoveAllFeatures(ge); // removes all loaded features from 'ge'
If you wish to remove a specific placemark, or any other feature, simply specify its ID as the parameter to the RemoveFeatureById method.
GEHelpers.RemoveFeatureById(ge, 'foo'); // remove the feature with the id 'foo'
An ID can be set either when you create the feature via the api or when you define the feature in kml. e.g.
// api
ge.createPlacemark('foo');
//kml id
<Document id="foo">
</Document>
Edit:
You should not have to do anything other than...
for (int i = 0; i < ge2.getFeatures().getChildNodes().getLength(); i++)
{
var kmlobject = ge2.getFeatures().getChildNodes().item[i];
if (kmlobject.getId().Contains("Line_"))
{
ge2.getFeatures().removeChild(kmlobject);
}
}
I think that there is possibly something else going on with your set up, maybe to do with having multiple instances of the plugin running at the same time.

problems with C# Port of Recast Detour Navigation Mesh for XNA, A* Pathing

Update 10/03
Ok, so i figured out the problem. C# is a funny old language..I changed
int navMeshCount = navMesh.GetPath(ref start, ref end, routePolys, navMeshRoute, false);
to
var navMeshCount = navMesh.GetPath(ref start, ref end, routePolys, navMeshRoute, false);
and finally im getting the pathfinding values to work!
When i debug the routeNavMesh array fills up with the values i require.
http://i.imgur.com/QgH8UL6.png [Screencap of debug in question]
Ill update again once im done.
Having some major problems getting a navigational mesh to work, in particular i cant get it to do any pathfinding or get it to respond to my vector3 input in any useful way.
Im working on the sunburn engine, it sits on top of XNA 4.0, so all of XNAs functions are still usable.
Recently Recast Navigation from digesting Duck's blog was ported over to c# and i've used it to create navigation meshes for my levels. Here is a link to the tutorial/guide i've been trying to follow: http://xboxforums.create.msdn.com/forums/p/109484/647991.aspx
im not sure how to pass in the start and end values on the getpath segment, are these meant to be values in world space or object space or something? If i do vector3 values and enter them as the start and end values( and then subsequently set them in a place on the array), i dont get an array of values that shows path finding, i just get those two verbatim values inside the navmeshroute array.
Here is where i try and run the path, its in my Update.
routePolys = new ushort[dtStatNavMesh.MAX_POLYS];
navMeshRoute = new Vector3[dtStatNavMesh.MAX_POLYS];
if (ks.IsKeyDown(Keys.L))
{
if (!runOnce)
{
start = new Vector3(0, 0, 0);
end = new Vector3(1000, 0, 1000);
ppos = x;
navMeshRoute[0].X = start.X;
navMeshRoute[0].Y = start.Y;
navMeshRoute[0].Z = start.Z;
navMeshRoute[count].X = end.X;
navMeshRoute[count].Y = end.Y;
navMeshRoute[count].Z = end.Z;
int navMeshStart = 0;
navMesh.GetPath(ref start, ref end, routePolys, navMeshRoute, true);
Console.WriteLine("navMeshPC" + navMesh.getPolyCount());
int navMeshCount = navMesh.GetPath(ref start, ref end, routePolys, navMeshRoute, true);
while (navMeshStart < navMeshCount)
{
routeNode = navMeshRoute[navMeshStart++];
}
Console.Out.WriteLine("NMc : " + navMeshCount);
}
}

Taking input from a joystick with C# .NET

I searched around on Google for this, but the only things I came up with were outdated and did not work.
Does anyone have any information on how to get joystick data using C# .NET?

Since this was the top hit I got on google while researching joystick / gamepad input in C#, I thought I should post a response for others to see.
The easiest way I found was to use SharpDX and DirectInput. You can install it via NuGet (SharpDX.DirectInput)
After that, it's simply a matter of calling a few methods:
Sample code from SharpDX
static void Main()
{
// Initialize DirectInput
var directInput = new DirectInput();
// Find a Joystick Guid
var joystickGuid = Guid.Empty;
foreach (var deviceInstance in directInput.GetDevices(DeviceType.Gamepad,
DeviceEnumerationFlags.AllDevices))
joystickGuid = deviceInstance.InstanceGuid;
// If Gamepad not found, look for a Joystick
if (joystickGuid == Guid.Empty)
foreach (var deviceInstance in directInput.GetDevices(DeviceType.Joystick,
DeviceEnumerationFlags.AllDevices))
joystickGuid = deviceInstance.InstanceGuid;
// If Joystick not found, throws an error
if (joystickGuid == Guid.Empty)
{
Console.WriteLine("No joystick/Gamepad found.");
Console.ReadKey();
Environment.Exit(1);
}
// Instantiate the joystick
var joystick = new Joystick(directInput, joystickGuid);
Console.WriteLine("Found Joystick/Gamepad with GUID: {0}", joystickGuid);
// Query all suported ForceFeedback effects
var allEffects = joystick.GetEffects();
foreach (var effectInfo in allEffects)
Console.WriteLine("Effect available {0}", effectInfo.Name);
// Set BufferSize in order to use buffered data.
joystick.Properties.BufferSize = 128;
// Acquire the joystick
joystick.Acquire();
// Poll events from joystick
while (true)
{
joystick.Poll();
var datas = joystick.GetBufferedData();
foreach (var state in datas)
Console.WriteLine(state);
}
}
I hope this helps.
I even got this to work with a DualShock3 and the MotioninJoy drivers.

One: use SlimDX.
Two: it looks something like this (where GamepadDevice is my own wrapper, and the code is slimmed down to just the relevant parts).
Find the joystick / pad GUIDs:
public virtual IList<GamepadDevice> Available()
{
IList<GamepadDevice> result = new List<GamepadDevice>();
DirectInput dinput = new DirectInput();
foreach (DeviceInstance di in dinput.GetDevices(DeviceClass.GameController, DeviceEnumerationFlags.AttachedOnly))
{
GamepadDevice dev = new GamepadDevice();
dev.Guid = di.InstanceGuid;
dev.Name = di.InstanceName;
result.Add(dev);
}
return result;
}
Once the user has selected from the list, acquire the gamepad:
private void acquire(System.Windows.Forms.Form parent)
{
DirectInput dinput = new DirectInput();
pad = new Joystick(dinput, this.Device.Guid);
foreach (DeviceObjectInstance doi in pad.GetObjects(ObjectDeviceType.Axis))
{
pad.GetObjectPropertiesById((int)doi.ObjectType).SetRange(-5000, 5000);
}
pad.Properties.AxisMode = DeviceAxisMode.Absolute;
pad.SetCooperativeLevel(parent, (CooperativeLevel.Nonexclusive | CooperativeLevel.Background));
pad.Acquire();
}
Polling the pad looks like this:
JoystickState state = new JoystickState();
if (pad.Poll().IsFailure)
{
result.Disconnect = true;
return result;
}
if (pad.GetCurrentState(ref state).IsFailure)
{
result.Disconnect = true;
return result;
}
result.X = state.X / 5000.0f;
result.Y = state.Y / 5000.0f;
int ispressed = 0;
bool[] buttons = state.GetButtons();

The bad news is that Microsoft seems to stop supporting their NET libraries for DirectX and focus on XNA instead. I don't work in GameDev so I don't need to use XNA but you may try it if you developing computer games. The good news is that there are other approaches. One is SlimDX the new framework to help you to wok with DirectX from C#. The other way is to directly add references of "Microsoft.DirectX.dll" and "Microsoft.DirectX.DirectInput.dll" to your project. you can find them "..\Windows\Microsoft.NET\DirectX for Managed Code\". if you you are going to use last approach here is a link to codeproject where you can read how to work with a joystick.
EDIT:
If your application is based on NET version newer then 2.0 the application may hang on. To fix this problem change config file and add this:
<startup useLegacyV2RuntimeActivationPolicy="true">

Google led me here and while not a requirement of this question, OpenTK is a good option for Windows and Linux (under mono) support.
From the OpenTK docs, this code works on Raspberry Pi + Raspbian + Mono 3.12.0:
for (int i = 0; i < 4; i++)
{
var state = Joystick.GetState(i);
if (state.IsConnected)
{
float x = state.GetAxis(JoystickAxis.Axis0);
float y = state.GetAxis(JoystickAxis.Axis1);
// Print the current state of the joystick
Console.WriteLine(state);
}
}

This question is old, but it seems to be active even to this day, so I'm posting anyway.
If you need to get input from XInput only, take a look at XInputium. This is a .NET library that is specialized in XInput controllers. It is pretty straightforward, and has many code samples you can look at.