public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
object __thing;
int piecesize = 0;
IntPtr h;
//point
point = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
//transient
transient = serializedMessage[currentIndex++] == 1;
//element_path
element_path = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
element_path = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
//display_name
display_name = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
display_name = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//waypoints
arraylength = rand.Next(10);
if (waypoints == null)
{
waypoints = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref waypoints, arraylength);
}
for (int i = 0; i < waypoints.Length; i++)
{
//waypoints[i]
waypoints[i] = new Messages.geometry_msgs.PoseStamped();
waypoints[i].Randomize();
}
//num_steps
num_steps = (uint)rand.Next();
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
object __thing;
//path
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (path == null)
{
path = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref path, arraylength);
}
for (int i = 0; i < path.Length; i++)
{
//path[i]
path[i] = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
}
//error_code
error_code = new Messages.moveit_msgs.MoveItErrorCodes(serializedMessage, ref currentIndex);
}
bool GetPathService(GetPath.Request req, ref GetPath.Response resp)
{
Debug.Log("getpath service called!");
Path path = new Path();
path.header = new Messages.std_msgs.Header();
path.header.Frame_id = "global";
path.header.Stamp = ROS.GetTime();
path.header.Seq = 0;
Pathing.PathSample[] samples = PathPlanner.GetPath();
int count = samples.Length;
path.poses = new PoseStamped[count];
Debug.Log("sending " + count + " samples");
for (int i = 0; i < count; i++)
{
PoseStamped pst = new PoseStamped();
pst.header = new Messages.std_msgs.Header();
pst.header.Frame_id = "local";
pst.header.Stamp = ROS.GetTime();
pst.header.Seq = (uint)i;
pst.pose = new Messages.geometry_msgs.Pose();
pst.pose.position = new Messages.geometry_msgs.Point(samples [i].position.ToRos());
pst.pose.orientation = new Messages.geometry_msgs.Quaternion(samples [i].orientation.ToRos());
path.poses [i] = pst;
}
resp.path = path;
return(true);
}
void PublishAll()
{
int sleep = 1000 / 60;
while (ROS.ok && !ROS.shutting_down)
{
PoseStamped ps = new PoseStamped();
ps.pose = new Messages.geometry_msgs.Pose();
ps.pose.position = new Messages.geometry_msgs.Point(droneController.Position.ToRos());
ps.pose.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
posePub.publish(ps);
Imu imu = new Imu();
imu.angular_velocity_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.linear_acceleration_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.orientation_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.angular_velocity = new GVector3(droneController.AngularVelocity.ToRos());
imu.linear_acceleration = new GVector3(droneController.LinearAcceleration.ToRos());
imu.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
imuPub.publish(imu);
Thread.Sleep(sleep);
}
}
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;
IntPtr ptr;
int x__size;
//poses
hasmetacomponents |= false;
if (poses == null)
{
poses = new Messages.geometry_msgs.PoseStamped[0];
}
pieces.Add(BitConverter.GetBytes(poses.Length));
for (int i = 0; i < poses.Length; i++)
{
//poses[i]
if (poses[i] == null)
{
poses[i] = new Messages.geometry_msgs.PoseStamped();
}
pieces.Add(poses[i].Serialize(true));
}
//link_names
hasmetacomponents |= false;
if (link_names == null)
{
link_names = new string[0];
}
pieces.Add(BitConverter.GetBytes(link_names.Length));
for (int i = 0; i < link_names.Length; i++)
{
//link_names[i]
if (link_names[i] == null)
{
link_names[i] = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)link_names[i]);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
}
// 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 override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//points
arraylength = rand.Next(10);
if (points == null)
{
points = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref points, arraylength);
}
for (int i = 0; i < points.Length; i++)
{
//points[i]
points[i] = new Messages.geometry_msgs.PoseStamped();
points[i].Randomize();
}
}
private static void GetIK(NodeHandle node, gm.PoseStamped poseStamped, string group, double ikTimeout = 1.0, int ikAttempts = 0, bool avoidCollisions = false)
{
Console.WriteLine("GetIK");
moveItMsgs.GetPositionIK.Request req = new moveItMsgs.GetPositionIK.Request();
req.ik_request = new moveItMsgs.PositionIKRequest();
Console.WriteLine(req.ik_request);
req.ik_request.group_name = group;
req.ik_request.pose_stamped = poseStamped;
req.ik_request.timeout.data = new TimeData(1, 0); // one second
req.ik_request.attempts = ikAttempts;
req.ik_request.avoid_collisions = avoidCollisions;
moveItMsgs.GetPositionIK.Response resp = new moveItMsgs.GetPositionIK.Response();
DateTime before = DateTime.Now;
Console.WriteLine("node.serviceClient");
bool res = node.serviceClient <moveItMsgs.GetPositionIK.Request, moveItMsgs.GetPositionIK.Response>("/compute_ik").call(req, ref resp);
if (res)
{
Console.WriteLine("got result");
Console.WriteLine(resp.error_code.val);
}
else
{
Console.WriteLine("FAILED to receive respond from service");
}
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//point
point = new Messages.geometry_msgs.PoseStamped();
point.Randomize();
//success
success = rand.Next(2) == 1;
//error
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
error = Encoding.ASCII.GetString(strbuf);
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//path
arraylength = rand.Next(10);
if (path == null)
{
path = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref path, arraylength);
}
for (int i = 0; i < path.Length; i++)
{
//path[i]
path[i] = new Messages.geometry_msgs.PoseStamped();
path[i].Randomize();
}
//error_code
error_code = new Messages.moveit_msgs.MoveItErrorCodes();
error_code.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;
IntPtr ptr;
int x__size;
//point
if (point == null)
{
point = new Messages.geometry_msgs.PoseStamped();
}
pieces.Add(point.Serialize(true));
//transient
thischunk = new byte[1];
thischunk[0] = (byte)((bool)transient ? 1 : 0);
pieces.Add(thischunk);
//element_path
if (element_path == null)
{
element_path = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)element_path);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
//display_name
if (display_name == null)
{
display_name = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)display_name);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
//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);
}
static void Main(string[] args)
{
ROS.Init(args, "MoveitTest");
NodeHandle node = new NodeHandle();
gm.PoseStamped result = SetPose(-0.1, 0.1, 0.2, 0.0, 180.0, 0.0, "");
Console.WriteLine("result");
Console.WriteLine(result);
GetIK(node, result, "endeffector");
ROS.waitForShutdown();
}
private static gm.PoseStamped SetPose(double x, double y, double z, double roll, double pitch, double yaw, string frameID)
{
gm.PoseStamped ps = new gm.PoseStamped();
Console.WriteLine(ps.pose);
ps.pose.position.x = x;
ps.pose.position.y = y;
ps.pose.position.z = z;
ps.pose.orientation = SetQuaternionFromRPY(Deg2Rad(roll), Deg2Rad(pitch), Deg2Rad(yaw));
ps.header.frame_id = frameID;
return(ps);
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//poses
arraylength = rand.Next(10);
if (poses == null)
{
poses = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref poses, arraylength);
}
for (int i = 0; i < poses.Length; i++)
{
//poses[i]
poses[i] = new Messages.geometry_msgs.PoseStamped();
poses[i].Randomize();
}
//link_names
arraylength = rand.Next(10);
if (link_names == null)
{
link_names = new string[arraylength];
}
else
{
Array.Resize(ref link_names, arraylength);
}
for (int i = 0; i < link_names.Length; i++)
{
//link_names[i]
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
link_names[i] = Encoding.ASCII.GetString(strbuf);
}
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//poses
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (poses == null)
{
poses = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref poses, arraylength);
}
for (int i = 0; i < poses.Length; i++)
{
//poses[i]
poses[i] = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
}
//link_names
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (link_names == null)
{
link_names = new string[arraylength];
}
else
{
Array.Resize(ref link_names, arraylength);
}
for (int i = 0; i < link_names.Length; i++)
{
//link_names[i]
link_names[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
link_names[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
}
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;
IntPtr ptr;
int x__size;
//waypoints
hasmetacomponents |= true;
if (waypoints == null)
{
waypoints = new Messages.geometry_msgs.PoseStamped[0];
}
pieces.Add(BitConverter.GetBytes(waypoints.Length));
for (int i = 0; i < waypoints.Length; i++)
{
//waypoints[i]
if (waypoints[i] == null)
{
waypoints[i] = new Messages.geometry_msgs.PoseStamped();
}
pieces.Add(waypoints[i].Serialize(true));
}
//num_steps
scratch1 = new byte[Marshal.SizeOf(typeof(uint))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(num_steps, 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);
}
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;
IntPtr ptr;
int x__size;
//path
hasmetacomponents |= false;
if (path == null)
{
path = new Messages.geometry_msgs.PoseStamped[0];
}
pieces.Add(BitConverter.GetBytes(path.Length));
for (int i = 0; i < path.Length; i++)
{
//path[i]
if (path[i] == null)
{
path[i] = new Messages.geometry_msgs.PoseStamped();
}
pieces.Add(path[i].Serialize(true));
}
//error_code
if (error_code == null)
{
error_code = new Messages.moveit_msgs.MoveItErrorCodes();
}
pieces.Add(error_code.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);
}
/// <summary>
/// Left nous button is pressed
/// </summary>
/// <param name="drawArea"></param>
/// <param name="e"></param>
public override void OnMouseDown(DrawArea drawArea, MouseEventArgs e)
{
//delete last goal draw object
if (goalDrawID != -1)
{
int al = drawArea.TheLayers.ActiveLayerIndex;
drawArea.TheLayers[al].Graphics.DeleteObjectByID(goalDrawID);
}
Point p = drawArea.BackTrackMouse(new Point(e.X, e.Y));
if (drawArea.CurrentPen == null)
{
DrawEllipse drawGoal = new DrawEllipse(p.X, p.Y, 4, 4, drawArea.LineColor, drawArea.FillColor, drawArea.DrawFilled, drawArea.LineWidth);
AddNewObject(drawArea, drawGoal);
goalDrawID = drawGoal.ID;
}
else
{
DrawEllipse drawGoal = new DrawEllipse(p.X, p.Y, 4, 4, drawArea.PenType, drawArea.FillColor, drawArea.DrawFilled);
AddNewObject(drawArea, drawGoal);
goalDrawID = drawGoal.ID;
}
PointF mapPoint = drawArea.World2Map(p);
gm.PoseStamped pose = new Messages.geometry_msgs.PoseStamped();
pose.header = new m.Header();
pose.header.frame_id = "map";
pose.header.stamp = Ros_CSharp.ROS.GetTime();
pose.pose = new gm.Pose();
pose.pose.position = new gm.Point();
pose.pose.position.x = mapPoint.X;
pose.pose.position.y = mapPoint.Y;
//emQuaternion quaternion= tf.net.emQuaternion.FromRPY(new emVector3(0,90, 0));
//pose.pose.orientation = quaternion.ToMsg();
pose.pose.orientation = new gm.Quaternion();
pose.pose.orientation.x = 0;
pose.pose.orientation.y = 0;
pose.pose.orientation.z = 0;
pose.pose.orientation.w = 1;
goalPub.publish(pose);
}
public void AddPose(PoseStamped ps)
{
if (!isStarted)
{
return;
}
if (startTime == 0)
{
startTime = ps.header.Stamp.data.toSec();
return;
}
testData.Add(new double[] {
ps.header.Stamp.data.toSec(),
ps.pose.position.z
});
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
object __thing;
int piecesize = 0;
IntPtr h;
//waypoints
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (waypoints == null)
{
waypoints = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref waypoints, arraylength);
}
for (int i = 0; i < waypoints.Length; i++)
{
//waypoints[i]
waypoints[i] = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
}
//num_steps
piecesize = Marshal.SizeOf(typeof(uint));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
num_steps = (uint)Marshal.PtrToStructure(h, typeof(uint));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
object __thing;
//point
point = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
//success
success = serializedMessage[currentIndex++] == 1;
//error
error = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
error = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
public void UpdatePose(PoseStamped ps)
{
if (!firstPoseReceived)
{
firstPoseReceived = true;
SetGoal(ps.pose.position);
}
Vec3 rpyCmd = new Vec3();
double radian = System.Math.PI / 180;
double t = ps.header.Stamp.data.toSec();
// Control Roll to to move along Y
double rollCmd = xController.Update(ps.pose.position.x, t);
rollCmd = Clamp(rollCmd, -10.0 * radian, 10.0 * radian);
// Control Pitch to move along X
double pitchCmd = yController.Update(ps.pose.position.y, t);
pitchCmd = Clamp(pitchCmd, -10.0 * radian, 10.0 * radian);
// Control Thrust to move along Z
double thrust = zController.Update(ps.pose.position.z, t);
rpyCmd.x = rollCmd;
rpyCmd.y = pitchCmd;
string s = string.Format("r: {0}, p: {1}, thrust: {2}", rollCmd, pitchCmd, thrust);
Debug.Log(s);
// publish
torqueCallback(rpyCmd);
thrustCallback(thrust);
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
object __thing;
int piecesize = 0;
IntPtr h;
//group_name
group_name = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
group_name = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
//joint_names
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (joint_names == null)
{
joint_names = new string[arraylength];
}
else
{
Array.Resize(ref joint_names, arraylength);
}
for (int i = 0; i < joint_names.Length; i++)
{
//joint_names[i]
joint_names[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
joint_names[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
//end_effector_link
end_effector_link = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
end_effector_link = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
//seed
seed = new Messages.xamlamoveit_msgs.JointPathPoint(serializedMessage, ref currentIndex);
//const_seed
const_seed = serializedMessage[currentIndex++] == 1;
//points
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (points == null)
{
points = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref points, arraylength);
}
for (int i = 0; i < points.Length; i++)
{
//points[i]
points[i] = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
}
//collision_check
collision_check = serializedMessage[currentIndex++] == 1;
//attempts
piecesize = Marshal.SizeOf(typeof(int));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
attempts = (int)Marshal.PtrToStructure(h, typeof(int));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//timeout
timeout = new Duration(new TimeData(
BitConverter.ToInt32(serializedMessage, currentIndex),
BitConverter.ToInt32(serializedMessage, currentIndex + Marshal.SizeOf(typeof(System.Int32)))));
currentIndex += 2 * Marshal.SizeOf(typeof(System.Int32));
}
void OldPublishAll()
{
// pose info
PoseStamped ps = new PoseStamped();
// ps.header = new Messages.std_msgs.Header ();
// ps.header.Stamp = ROS.GetTime ();
// ps.header.Frame_id = "";
ps.pose = new Messages.geometry_msgs.Pose();
Imu imu = new Imu();
// imu info
// imu.header = new Messages.std_msgs.Header ( ps.header );
imu.angular_velocity_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.linear_acceleration_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
imu.orientation_covariance = new double[9] {
-1, 0, 0, 0, 0, 0, 0, 0, 0
};
// image info
Image img = new Image();
// img.header = new Messages.std_msgs.Header ( ps.header );
img.width = (uint)QuadMotor.ImageWidth;
img.height = (uint)QuadMotor.ImageHeight;
img.encoding = "mono16"; // "rgba8";
img.step = img.width * 2; // * 4
img.is_bigendian = 1;
// int sleep = 1000 / 30;
// int sleep = 1000 / 2;
int sleep = 1000 / 60;
// int sleep = 1000 / 120;
// int sleep = 1000 / 250;
#if TIMETEST
Queue <TimeData> tdq = new Queue <TimeData> ();
Queue <long> tq = new Queue <long> ();
#endif
while (ROS.ok && !ROS.shutting_down)
{
#if TIMETEST
tdq.Enqueue(ROS.GetTime().data);
tq.Enqueue(System.DateTime.Now.Ticks);
#endif
// publish pose
///*
// ps.header = new Messages.std_msgs.Header ();
// ps.header.Stamp = ROS.GetTime ();
// ps.header.Frame_id = ps.header.Stamp.data.toSec ().ToString ();
// ps.header.Frame_id = "";
// ps.header.Seq = frameSeq;
ps.pose.position = new Messages.geometry_msgs.Point(droneController.Position.ToRos());
ps.pose.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
posePub.publish(ps);
if (ps.header != null)
{
Debug.Log("Send Time " + ps.header.Stamp.data.toSec());
}
else
{
Debug.Log("Send time null header");
}
// */
// publish imu
///*
// imu.header = new Messages.std_msgs.Header ();
// imu.header.Frame_id = "";
// imu.header.Seq = frameSeq++;
// imu.header.Stamp = ROS.GetTime ();
// imu.header.Stamp = ps.header.Stamp;
imu.angular_velocity = new GVector3(droneController.AngularVelocity.ToRos());
imu.linear_acceleration = new GVector3(droneController.LinearAcceleration.ToRos());
imu.orientation = new Messages.geometry_msgs.Quaternion(droneController.Rotation.ToRos());
imuPub.publish(imu);
// Debug.Log ( "Send Imu " + imu.header.Seq );
// publish image
// img.data = droneController.GetImageData ();
// imgPub.publish ( img );
// */
Thread.Sleep(sleep);
}
#if TIMETEST
while (tdq.Count > 0)
{
TimeData td = tdq.Dequeue();
Debug.Log("t: " + td.sec + " " + td.nsec);
}
while (tq.Count > 0)
{
Debug.Log("ticks: " + tq.Dequeue());
}
#endif
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//group_name
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
group_name = Encoding.ASCII.GetString(strbuf);
//joint_names
arraylength = rand.Next(10);
if (joint_names == null)
{
joint_names = new string[arraylength];
}
else
{
Array.Resize(ref joint_names, arraylength);
}
for (int i = 0; i < joint_names.Length; i++)
{
//joint_names[i]
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
joint_names[i] = Encoding.ASCII.GetString(strbuf);
}
//end_effector_link
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
end_effector_link = Encoding.ASCII.GetString(strbuf);
//seed
seed = new Messages.xamlamoveit_msgs.JointPathPoint();
seed.Randomize();
//const_seed
const_seed = rand.Next(2) == 1;
//points
arraylength = rand.Next(10);
if (points == null)
{
points = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref points, arraylength);
}
for (int i = 0; i < points.Length; i++)
{
//points[i]
points[i] = new Messages.geometry_msgs.PoseStamped();
points[i].Randomize();
}
//collision_check
collision_check = rand.Next(2) == 1;
//attempts
attempts = rand.Next();
//timeout
timeout = new Duration(new TimeData(
Convert.ToInt32(rand.Next()),
Convert.ToInt32(rand.Next())));
}
public override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
object __thing;
//points
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (points == null)
{
points = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref points, arraylength);
}
for (int i = 0; i < points.Length; i++)
{
//points[i]
points[i] = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
}
//names
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (names == null)
{
names = new string[arraylength];
}
else
{
Array.Resize(ref names, arraylength);
}
for (int i = 0; i < names.Length; i++)
{
//names[i]
names[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
names[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
//element_paths
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (element_paths == null)
{
element_paths = new string[arraylength];
}
else
{
Array.Resize(ref element_paths, arraylength);
}
for (int i = 0; i < element_paths.Length; i++)
{
//element_paths[i]
element_paths[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
element_paths[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
//success
success = serializedMessage[currentIndex++] == 1;
//error
error = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
error = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//end_effector_name
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
end_effector_name = Encoding.ASCII.GetString(strbuf);
//waypoints
arraylength = rand.Next(10);
if (waypoints == null)
{
waypoints = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref waypoints, arraylength);
}
for (int i = 0; i < waypoints.Length; i++)
{
//waypoints[i]
waypoints[i] = new Messages.geometry_msgs.PoseStamped();
waypoints[i].Randomize();
}
//max_xyz_velocity
max_xyz_velocity = (rand.Next() + rand.NextDouble());
//max_xyz_acceleration
max_xyz_acceleration = (rand.Next() + rand.NextDouble());
//max_angular_velocity
max_angular_velocity = (rand.Next() + rand.NextDouble());
//max_angular_acceleration
max_angular_acceleration = (rand.Next() + rand.NextDouble());
//dt
dt = (rand.Next() + rand.NextDouble());
//ik_jump_threshold
ik_jump_threshold = (rand.Next() + rand.NextDouble());
//max_deviation
max_deviation = (rand.Next() + rand.NextDouble());
//joint_names
arraylength = rand.Next(10);
if (joint_names == null)
{
joint_names = new string[arraylength];
}
else
{
Array.Resize(ref joint_names, arraylength);
}
for (int i = 0; i < joint_names.Length; i++)
{
//joint_names[i]
strlength = rand.Next(100) + 1;
strbuf = new byte[strlength];
rand.NextBytes(strbuf); //fill the whole buffer with random bytes
for (int __x__ = 0; __x__ < strlength; __x__++)
{
if (strbuf[__x__] == 0) //replace null chars with non-null random ones
{
strbuf[__x__] = (byte)(rand.Next(254) + 1);
}
}
strbuf[strlength - 1] = 0; //null terminate
joint_names[i] = Encoding.ASCII.GetString(strbuf);
}
//seed
seed = new Messages.xamlamoveit_msgs.JointPathPoint();
seed.Randomize();
//collision_check
collision_check = rand.Next(2) == 1;
}
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;
IntPtr ptr;
int x__size;
//end_effector_name
if (end_effector_name == null)
{
end_effector_name = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)end_effector_name);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
//waypoints
hasmetacomponents |= true;
if (waypoints == null)
{
waypoints = new Messages.geometry_msgs.PoseStamped[0];
}
pieces.Add(BitConverter.GetBytes(waypoints.Length));
for (int i = 0; i < waypoints.Length; i++)
{
//waypoints[i]
if (waypoints[i] == null)
{
waypoints[i] = new Messages.geometry_msgs.PoseStamped();
}
pieces.Add(waypoints[i].Serialize(true));
}
//max_xyz_velocity
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(max_xyz_velocity, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//max_xyz_acceleration
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(max_xyz_acceleration, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//max_angular_velocity
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(max_angular_velocity, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//max_angular_acceleration
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(max_angular_acceleration, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//dt
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(dt, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//ik_jump_threshold
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(ik_jump_threshold, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//max_deviation
scratch1 = new byte[Marshal.SizeOf(typeof(double))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(max_deviation, h.AddrOfPinnedObject(), false);
h.Free();
pieces.Add(scratch1);
//joint_names
hasmetacomponents |= false;
if (joint_names == null)
{
joint_names = new string[0];
}
pieces.Add(BitConverter.GetBytes(joint_names.Length));
for (int i = 0; i < joint_names.Length; i++)
{
//joint_names[i]
if (joint_names[i] == null)
{
joint_names[i] = "";
}
scratch1 = Encoding.ASCII.GetBytes((string)joint_names[i]);
thischunk = new byte[scratch1.Length + 4];
scratch2 = BitConverter.GetBytes(scratch1.Length);
Array.Copy(scratch1, 0, thischunk, 4, scratch1.Length);
Array.Copy(scratch2, thischunk, 4);
pieces.Add(thischunk);
}
//seed
if (seed == null)
{
seed = new Messages.xamlamoveit_msgs.JointPathPoint();
}
pieces.Add(seed.Serialize(true));
//collision_check
thischunk = new byte[1];
thischunk[0] = (byte)((bool)collision_check ? 1 : 0);
pieces.Add(thischunk);
//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 override void Deserialize(byte[] serializedMessage, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
object __thing;
int piecesize = 0;
IntPtr h;
//end_effector_name
end_effector_name = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
end_effector_name = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
//waypoints
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (waypoints == null)
{
waypoints = new Messages.geometry_msgs.PoseStamped[arraylength];
}
else
{
Array.Resize(ref waypoints, arraylength);
}
for (int i = 0; i < waypoints.Length; i++)
{
//waypoints[i]
waypoints[i] = new Messages.geometry_msgs.PoseStamped(serializedMessage, ref currentIndex);
}
//max_xyz_velocity
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
max_xyz_velocity = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//max_xyz_acceleration
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
max_xyz_acceleration = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//max_angular_velocity
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
max_angular_velocity = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//max_angular_acceleration
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
max_angular_acceleration = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//dt
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
dt = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//ik_jump_threshold
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
ik_jump_threshold = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//max_deviation
piecesize = Marshal.SizeOf(typeof(double));
h = IntPtr.Zero;
if (serializedMessage.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(serializedMessage, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Memory allocation failed");
}
max_deviation = (double)Marshal.PtrToStructure(h, typeof(double));
Marshal.FreeHGlobal(h);
currentIndex += piecesize;
//joint_names
hasmetacomponents |= false;
arraylength = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (joint_names == null)
{
joint_names = new string[arraylength];
}
else
{
Array.Resize(ref joint_names, arraylength);
}
for (int i = 0; i < joint_names.Length; i++)
{
//joint_names[i]
joint_names[i] = "";
piecesize = BitConverter.ToInt32(serializedMessage, currentIndex);
currentIndex += 4;
joint_names[i] = Encoding.ASCII.GetString(serializedMessage, currentIndex, piecesize);
currentIndex += piecesize;
}
//seed
seed = new Messages.xamlamoveit_msgs.JointPathPoint(serializedMessage, ref currentIndex);
//collision_check
collision_check = serializedMessage[currentIndex++] == 1;
}
public void UpdatePose(PoseStamped pose)
{
double zCmd = controller.Update(pose.pose.position.z, pose.header.Stamp.data.toSec());
thrustCallback(zCmd);
}
