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); }