public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//header
header = new Header(SERIALIZEDSTUFF, ref currentIndex);
//poses
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (poses == null)
{
poses = new PoseStamped[arraylength];
}
else
{
Array.Resize(ref poses, arraylength);
}
for (int i = 0; i < poses.Length; i++)
{
//poses[i]
poses[i] = new PoseStamped(SERIALIZEDSTUFF, ref currentIndex);
}
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//header
header = new Header();
header.Randomize();
//poses
arraylength = rand.Next(10);
if (poses == null)
{
poses = new PoseStamped[arraylength];
}
else
{
Array.Resize(ref poses, arraylength);
}
for (int i = 0; i < poses.Length; i++)
{
//poses[i]
poses[i] = new PoseStamped();
poses[i].Randomize();
}
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
object __thing;
int piecesize = 0;
IntPtr h;
//start
start = new PoseStamped(SERIALIZEDSTUFF, ref currentIndex);
//goal
goal = new PoseStamped(SERIALIZEDSTUFF, ref currentIndex);
//tolerance
piecesize = Marshal.SizeOf(typeof(Single));
h = IntPtr.Zero;
if (SERIALIZEDSTUFF.Length - currentIndex != 0)
{
h = Marshal.AllocHGlobal(piecesize);
Marshal.Copy(SERIALIZEDSTUFF, currentIndex, h, piecesize);
}
if (h == IntPtr.Zero)
{
throw new Exception("Alloc failed");
}
tolerance = (Single)Marshal.PtrToStructure(h, typeof(Single));
Marshal.FreeHGlobal(h);
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;
//base_position
if (base_position == null)
{
base_position = new PoseStamped();
}
pieces.Add(base_position.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);
}
void PoseCallback(PoseStamped poseInfo)
{
Position = poseInfo.pose.position.ToUnity();
Rotation = poseInfo.pose.orientation.ToUnity();
// Debug.Log ( "Pose " + poseInfo.header.Seq );
// Debug.Log ( "Pose " + poseInfo.header.Frame_id + " " + poseInfo.header.Stamp.data.toSec () );
// Debug.Log ( "Pose " + poseInfo.header.Stamp.data.toSec () );
}
public PlaceLocation(string id, JointTrajectory post_place_posture, PoseStamped place_pose, double quality, GripperTranslation pre_place_approach, GripperTranslation post_place_retreat, string[] allowed_touch_objects)
{
this.id = id;
this.post_place_posture = post_place_posture;
this.place_pose = place_pose;
this.quality = quality;
this.pre_place_approach = pre_place_approach;
this.post_place_retreat = post_place_retreat;
this.allowed_touch_objects = allowed_touch_objects;
}
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;
//pose_stamp
hasmetacomponents |= true;
if (pose_stamp == null)
{
pose_stamp = new PoseStamped[0];
}
pieces.Add(BitConverter.GetBytes(pose_stamp.Length));
for (int i = 0; i < pose_stamp.Length; i++)
{
//pose_stamp[i]
if (pose_stamp[i] == null)
{
pose_stamp[i] = new PoseStamped();
}
pieces.Add(pose_stamp[i].Serialize(true));
}
//seed_angles
hasmetacomponents |= true;
if (seed_angles == null)
{
seed_angles = new Messages.sensor_msgs.JointState[0];
}
pieces.Add(BitConverter.GetBytes(seed_angles.Length));
for (int i = 0; i < seed_angles.Length; i++)
{
//seed_angles[i]
if (seed_angles[i] == null)
{
seed_angles[i] = new Messages.sensor_msgs.JointState();
}
pieces.Add(seed_angles[i].Serialize(true));
}
//seed_mode
pieces.Add(new[] { (byte)seed_mode });
//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);
}
protected override void Handle(PoseStamped message)
{
if (Container?.Count == 0)
{
Container.Add(message.GetPose() !.ToTracked());
}
else
{
Container?.Update(message.GetPose() !.ToTracked());
}
}
public VisibilityConstraint(double target_radius, PoseStamped target_pose, int cone_sides, PoseStamped sensor_pose, double max_view_angle, double max_range_angle, byte sensor_view_direction, double weight)
{
this.target_radius = target_radius;
this.target_pose = target_pose;
this.cone_sides = cone_sides;
this.sensor_pose = sensor_pose;
this.max_view_angle = max_view_angle;
this.max_range_angle = max_range_angle;
this.sensor_view_direction = sensor_view_direction;
this.weight = weight;
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//base_position
base_position = new PoseStamped();
base_position.Randomize();
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//target_pose
target_pose = new PoseStamped();
target_pose.Randomize();
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//base_position
base_position = new PoseStamped(SERIALIZEDSTUFF, ref currentIndex);
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
object __thing;
int piecesize = 0;
IntPtr h;
//pose_stamp
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (pose_stamp == null)
{
pose_stamp = new PoseStamped[arraylength];
}
else
{
Array.Resize(ref pose_stamp, arraylength);
}
for (int i = 0; i < pose_stamp.Length; i++)
{
//pose_stamp[i]
pose_stamp[i] = new PoseStamped(SERIALIZEDSTUFF, ref currentIndex);
}
//seed_angles
hasmetacomponents |= true;
arraylength = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex);
currentIndex += Marshal.SizeOf(typeof(System.Int32));
if (seed_angles == null)
{
seed_angles = new Messages.sensor_msgs.JointState[arraylength];
}
else
{
Array.Resize(ref seed_angles, arraylength);
}
for (int i = 0; i < seed_angles.Length; i++)
{
//seed_angles[i]
seed_angles[i] = new Messages.sensor_msgs.JointState(SERIALIZEDSTUFF, ref currentIndex);
}
//seed_mode
seed_mode = SERIALIZEDSTUFF[currentIndex++];
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//start
start = new PoseStamped();
start.Randomize();
//goal
goal = new PoseStamped();
goal.Randomize();
//tolerance
tolerance = (float)(rand.Next() + rand.NextDouble());
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//pose_stamp
arraylength = rand.Next(10);
if (pose_stamp == null)
{
pose_stamp = new PoseStamped[arraylength];
}
else
{
Array.Resize(ref pose_stamp, arraylength);
}
for (int i = 0; i < pose_stamp.Length; i++)
{
//pose_stamp[i]
pose_stamp[i] = new PoseStamped();
pose_stamp[i].Randomize();
}
//seed_angles
arraylength = rand.Next(10);
if (seed_angles == null)
{
seed_angles = new Messages.sensor_msgs.JointState[arraylength];
}
else
{
Array.Resize(ref seed_angles, arraylength);
}
for (int i = 0; i < seed_angles.Length; i++)
{
//seed_angles[i]
seed_angles[i] = new Messages.sensor_msgs.JointState();
seed_angles[i].Randomize();
}
//seed_mode
myByte = new byte[1];
rand.NextBytes(myByte);
seed_mode = myByte[0];
}
PoseFeedback(PoseFeedback pf)
{
current_pose = new PoseStamped();
current_pose.pose = new Pose();
current_pose.pose.orientation = new Quaternion();
current_pose.pose.orientation.x = pf.current_pose.pose.orientation.x;
current_pose.pose.orientation.y = pf.current_pose.pose.orientation.y;
current_pose.pose.orientation.z = pf.current_pose.pose.orientation.z;
current_pose.pose.orientation.w = pf.current_pose.pose.orientation.w;
current_pose.pose.position = new Point();
current_pose.pose.position.x = pf.current_pose.pose.position.x;
current_pose.pose.position.y = pf.current_pose.pose.position.y;
current_pose.pose.position.z = pf.current_pose.pose.position.z;
current_pose.header = new Header();
current_pose.header.Frame_id = pf.current_pose.header.Frame_id;
current_pose.header.Seq = pf.current_pose.header.Seq;
current_pose.header.Stamp = new Time(pf.current_pose.header.Stamp.data);
}
TakeoffFeedback(TakeoffFeedback tf)
{
current_pose = new PoseStamped();
current_pose.pose = new Pose();
current_pose.pose.orientation = new Quaternion();
current_pose.pose.orientation.x = tf.current_pose.pose.orientation.x;
current_pose.pose.orientation.y = tf.current_pose.pose.orientation.y;
current_pose.pose.orientation.z = tf.current_pose.pose.orientation.z;
current_pose.pose.orientation.w = tf.current_pose.pose.orientation.w;
current_pose.pose.position = new Point();
current_pose.pose.position.x = tf.current_pose.pose.position.x;
current_pose.pose.position.y = tf.current_pose.pose.position.y;
current_pose.pose.position.z = tf.current_pose.pose.position.z;
current_pose.header = new Header();
current_pose.header.Frame_id = tf.current_pose.header.Frame_id;
current_pose.header.Seq = tf.current_pose.header.Seq;
current_pose.header.Stamp = new Time(tf.current_pose.header.Stamp.data);
}
PoseGoal(PoseGoal pg)
{
target_pose = new PoseStamped();
target_pose.pose = new Pose();
target_pose.pose.orientation = new Quaternion();
target_pose.pose.orientation.x = pg.target_pose.pose.orientation.x;
target_pose.pose.orientation.y = pg.target_pose.pose.orientation.y;
target_pose.pose.orientation.z = pg.target_pose.pose.orientation.z;
target_pose.pose.orientation.w = pg.target_pose.pose.orientation.w;
target_pose.pose.position = new Point();
target_pose.pose.position.x = pg.target_pose.pose.position.x;
target_pose.pose.position.y = pg.target_pose.pose.position.y;
target_pose.pose.position.z = pg.target_pose.pose.position.z;
target_pose.header = new Header();
target_pose.header.Frame_id = pg.target_pose.header.Frame_id;
target_pose.header.Seq = pg.target_pose.header.Seq;
target_pose.header.Stamp = new Time(pg.target_pose.header.Stamp.data);
}
LandingFeedback(LandingFeedback lf)
{
current_pose = new PoseStamped();
current_pose.pose = new Pose();
current_pose.pose.orientation = new Quaternion();
current_pose.pose.orientation.x = lf.current_pose.pose.orientation.x;
current_pose.pose.orientation.y = lf.current_pose.pose.orientation.y;
current_pose.pose.orientation.z = lf.current_pose.pose.orientation.z;
current_pose.pose.orientation.w = lf.current_pose.pose.orientation.w;
current_pose.pose.position = new Point();
current_pose.pose.position.x = lf.current_pose.pose.position.x;
current_pose.pose.position.y = lf.current_pose.pose.position.y;
current_pose.pose.position.z = lf.current_pose.pose.position.z;
current_pose.header = new Header();
current_pose.header.Frame_id = lf.current_pose.header.Frame_id;
current_pose.header.Seq = lf.current_pose.header.Seq;
current_pose.header.Stamp = new Time(lf.current_pose.header.Stamp.data);
}
public override byte[] Serialize(bool partofsomethingelse)
{
int currentIndex = 0, length = 0;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
List <byte[]> pieces = new List <byte[]>();
GCHandle h;
//header
if (header == null)
{
header = new Header();
}
pieces.Add(header.Serialize(true));
//poses
hasmetacomponents |= true;
if (poses == null)
{
poses = new 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 PoseStamped();
}
pieces.Add(poses[i].Serialize(true));
}
//combine every array in pieces into one array and return it
int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
int __a_b__e = 0;
byte[] __a_b__d = new byte[__a_b__f];
foreach (var __p__ in pieces)
{
Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
__a_b__e += __p__.Length;
}
return(__a_b__d);
}
public 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;
//start
if (start == null)
{
start = new PoseStamped();
}
pieces.Add(start.Serialize(true));
//goal
if (goal == null)
{
goal = new PoseStamped();
}
pieces.Add(goal.Serialize(true));
//tolerance
scratch1 = new byte[Marshal.SizeOf(typeof(Single))];
h = GCHandle.Alloc(scratch1, GCHandleType.Pinned);
Marshal.StructureToPtr(tolerance, 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);
}
private void SendRobotTargetPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition)
{
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
PoseStamped message = new PoseStamped();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = robotTargetPoseFrameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation);
message.pose.orientation.x = Quaternion.identity.x;
message.pose.orientation.y = Quaternion.identity.y;
message.pose.orientation.z = Quaternion.identity.z;
message.pose.orientation.w = Quaternion.identity.w;
message.pose.position.x = currentPosition.x;
message.pose.position.y = currentPosition.y;
message.pose.position.z = currentPosition.z;
robotTargetPosePublisher.Publish(message);
System.Diagnostics.Debug.WriteLine("ROBOT TARGET POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() +
" --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")");
}
public MoveBaseGoal()
{
this.target_pose = new PoseStamped();
}
public Grasp(string id, JointTrajectory pre_grasp_posture, JointTrajectory grasp_posture, PoseStamped grasp_pose, double grasp_quality, GripperTranslation pre_grasp_approach, GripperTranslation post_grasp_retreat, GripperTranslation post_place_retreat, float max_contact_force, string[] allowed_touch_objects)
{
this.id = id;
this.pre_grasp_posture = pre_grasp_posture;
this.grasp_posture = grasp_posture;
this.grasp_pose = grasp_pose;
this.grasp_quality = grasp_quality;
this.pre_grasp_approach = pre_grasp_approach;
this.post_grasp_retreat = post_grasp_retreat;
this.post_place_retreat = post_place_retreat;
this.max_contact_force = max_contact_force;
this.allowed_touch_objects = allowed_touch_objects;
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
current_pose = new PoseStamped(SERIALIZEDSTUFF, ref currentIndex);
}
public PositionIKRequest(string group_name, RobotState robot_state, Constraints constraints, bool avoid_collisions, string ik_link_name, PoseStamped pose_stamped, string[] ik_link_names, PoseStamped[] pose_stamped_vector, Duration timeout)
{
this.group_name = group_name;
this.robot_state = robot_state;
this.constraints = constraints;
this.avoid_collisions = avoid_collisions;
this.ik_link_name = ik_link_name;
this.pose_stamped = pose_stamped;
this.ik_link_names = ik_link_names;
this.pose_stamped_vector = pose_stamped_vector;
this.timeout = timeout;
}
void poseCallback (PoseStamped pose)
{
pose_ = pose;
}
public MoveBaseGoal(PoseStamped target_pose)
{
this.target_pose = target_pose;
}
public GetPlanRequest()
{
this.start = new PoseStamped();
this.goal = new PoseStamped();
this.tolerance = 0.0f;
}
public GetPlanRequest(PoseStamped start, PoseStamped goal, float tolerance)
{
this.start = start;
this.goal = goal;
this.tolerance = tolerance;
}
