private string serializeJointTrajectory(JointTrajectory jt)
{
//serialisiere Topic
string topic = "\"topic\": \"/preview_trajectory\", "; // platzhalter, topic vom RosMessage- objekt später holen
// serialisiere header
Header jtheader = jt.header;
string headerString = RosHeaderCoder_.serializeSingleHeader(jtheader);
//serialisiere joint_names
string[] jtjoint_names = jt.joint_names;
string joint_names_string = RosArrayService_.baueStringWertArrayUm(jtjoint_names, "\"joint_names\": ", false);
//serialisiere JoinTrajectoryPoints
JointTrajectoryPoint[] jtps = jt.points;
string points_string = "";
for (int i = 0; i < jtps.Length - 2; ++i)
{
points_string = points_string + RosJointTrajectoryPointCoder_.serializeSingleJointTrajectoryPoint(jtps[i]) + ", ";
}
points_string = points_string + RosJointTrajectoryPointCoder_.serializeSingleJointTrajectoryPoint(jtps[jtps.Length - 1]);
points_string = "\"points: \"[" + points_string + "]";
// Einzelteile zusammenkleben und formatiert zurückgeben
return("\"{" + topic + "\"msg\": {" + headerString + joint_names_string + points_string + "}, \"op\": \"publish\"}\"");
}
public FollowJointTrajectoryGoal(JointTrajectory trajectory, JointTolerance[] path_tolerance, JointTolerance[] goal_tolerance, Duration goal_time_tolerance)
{
this.trajectory = trajectory;
this.path_tolerance = path_tolerance;
this.goal_tolerance = goal_tolerance;
this.goal_time_tolerance = goal_time_tolerance;
}
private RosPublish deserializeJointTrajectory(JsonObject jsonObject)
{
JsonArray jjoint_names = jsonObject["msg"].GetObject()["joint_names"].GetArray();
// baue den Header
Header header = RosHeaderCoder_.deserializeSingleHeader((JsonValue)jsonObject["msg"].GetObject()["header"]);
// Lasse die JointTrajectoryPoints vom jeweiligen Coder deserialisieren
JsonArray jjointtrajectorypoints = jsonObject["msg"].GetObject()["points"].GetArray();
//TODO JointTrajectoryPoints deserialisieren
JointTrajectoryPoint[] pts = new JointTrajectoryPoint[jjointtrajectorypoints.Count];
for (int i = 0; i < jjointtrajectorypoints.Count; i++)
{
pts[i] = RosJointTrajectoryPointCoder_.deserializeSingleJointTrajectoryPoint((JsonValue)jjointtrajectorypoints[i]);
}
JointTrajectory messageData = new JointTrajectory();
messageData.header = header;
// Nutze den ArrayService, um die JSONArrays in "normale" Arrays umzuwandeln
messageData.joint_names = RosArrayService_.stringArrayAusJSonArray(jjoint_names);
messageData.points = pts;
return(new RosPublish("\"/preview_trajectory\"", messageData));
}
public SetJointTrajectoryRequest(string model_name, JointTrajectory joint_trajectory, Pose model_pose, bool set_model_pose, bool disable_physics_updates)
{
this.model_name = model_name;
this.joint_trajectory = joint_trajectory;
this.model_pose = model_pose;
this.set_model_pose = set_model_pose;
this.disable_physics_updates = disable_physics_updates;
}
// defaults to false
public SetJointTrajectoryRequest()
{
this.model_name = "";
this.joint_trajectory = new JointTrajectory();
this.model_pose = new Pose();
this.set_model_pose = false;
this.disable_physics_updates = false;
}
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 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 void TestInit()
{
var joints = new JointSet("a", "b", "c");
var p1 = GetPoint(100, joints);
var p2 = GetPoint(200, joints);
var p3 = GetPoint(300, joints);
var t = new JointTrajectory(joints, new JointTrajectoryPoint[] { p1, p2, p3 });
Assert.Equal(3, t.Count);
Assert.Equal(p1, t[0]);
Assert.Equal(p2, t[1]);
Assert.Equal(p3, t[2]);
Assert.Throws <ArgumentException>(() => new JointTrajectory(joints, new JointTrajectoryPoint[] { p1, p3, p2 }));
}
public static PyObject ToPython(this JointTrajectory obj)
{
var jointSet = PyConvert.ToPyObject(obj.JointSet);
using (Py.GIL())
{
var points = new PyList();
foreach (var p in obj)
{
points.Append(PyConvert.ToPyObject(p));
}
return(pyXamlaMotionTypes.JointTrajectory(jointSet, points));
}
}
public void TestEvaluateAt()
{
void AssertEqualPoints(JointTrajectoryPoint a, JointTrajectoryPoint b)
{
bool Compare(JointValues aa, JointValues bb)
{
double delta = Math.Abs(aa.MaxNorm() - bb.MaxNorm());
return(delta < 1E-6);
}
Assert.Equal(a.TimeFromStart, b.TimeFromStart);
Assert.True(a.JointSet == b.JointSet);
Assert.True(Compare(a.Positions, b.Positions));
Assert.True(Compare(a.Velocities, b.Velocities));
}
var joints = new JointSet("a", "b", "c");
JointTrajectoryPoint[] points = new JointTrajectoryPoint[10];
for (int i = 0; i < 10; ++i)
{
points[i] = GetPoint(i, joints);
}
Assert.Equal(points.Count(), 10);
var traj = new JointTrajectory(joints, points);
for (int i = 0; i < 50; ++i)
{
// Create negative and out of bound time values, to assure exceptions are thrown accordingly
double time = rng.NextDouble() * 12 - 2;
var index = (int)time;
var k = Math.Min(index + 1, 9);
var timeSpan = TimeSpan.FromSeconds(time);
if (time < 0 || time > 9)
{
Assert.Throws <ArgumentOutOfRangeException>(() => traj.EvaluateAt(timeSpan));
}
else
{
var pointGT = traj[index].InterpolateCubic(traj[k], TimeSpan.FromSeconds(time));
// time = Math.Min(time, 9);
// time = Math.Max(time, 0);
var pointEval = traj.EvaluateAt(timeSpan);
AssertEqualPoints(pointGT, pointEval);
}
}
}
public void TestConcat()
{
var joints = new JointSet("a", "b", "c");
var p1 = GetPoint(100, joints);
var p2 = GetPoint(200, joints);
var p3 = GetPoint(300, joints);
var t1 = new JointTrajectory(joints, new JointTrajectoryPoint[] { p1, p2 });
var t2 = new JointTrajectory(joints, new JointTrajectoryPoint[] { p3 });
var t = t1.Concat(t2);
Assert.Equal(3, t.Count);
Assert.Equal(p1, t[0]);
Assert.Equal(p2, t[1]);
// assert that duration of t1 gets added to t2
Assert.Equal(p3.WithTimeFromStart(TimeSpan.FromSeconds(500)), t[2]);
}
// die andere Richtung wäre auch schön, damit man auch Nachrichten an ROS senden kann
public string startSerializing(RosMessage message)
{
//todo
JointTrajectory jt = new JointTrajectory();
RosPublish publishMessage = (RosPublish)message;
MessageData msg = publishMessage.msg;
if (msg.GetType() == typeof(JointTrajectory))
{
jt = (JointTrajectory)msg;
}
return(serializeJointTrajectory(jt));
}
public void TestSub()
{
var joints = new JointSet("a", "b", "c");
var p1 = GetPoint(50, joints);
var p2 = GetPoint(200, joints);
var p3 = GetPoint(300, joints);
var p4 = GetPoint(800, joints);
var tBig = new JointTrajectory(joints, new JointTrajectoryPoint[] { p1, p2, p3, p4 });
var t = tBig.Sub(1, 3);
Assert.Equal(2, t.Count);
// assert correct duration
Assert.Equal(p2.WithTimeFromStart(TimeSpan.FromSeconds(200)), t[0]);
Assert.Equal(p3.WithTimeFromStart(TimeSpan.FromSeconds(300)), t[1]);
Assert.Throws <ArgumentOutOfRangeException>(() => tBig.Sub(0, tBig.Count));
Assert.Throws <ArgumentOutOfRangeException>(() => tBig.Sub(-1, tBig.Count - 1));
}
public void TestMerge()
{
var timeSpan = TimeSpan.FromSeconds(100);
var jointsA = new JointSet("a", "b", "c");
var jointsB = new JointSet("e", "f");
var pA1 = GetPoint(0, jointsA);
var pA2 = GetPoint(200, jointsA);
var pA3 = GetPoint(300, jointsA);
var pB1 = GetPoint(0, jointsB);
var pB2 = GetPoint(200, jointsB);
var pB3 = GetPoint(300, jointsB);
var t1 = new JointTrajectory(jointsA, new JointTrajectoryPoint[] { pA1, pA2, pA3 });
var t2 = new JointTrajectory(jointsB, new JointTrajectoryPoint[] { pB1, pB2, pB3 });
var t = t1.Merge(t2);
Assert.True(Math.Abs(t[1].TimeFromStartSeconds - 150) < 1E-9);
}
public RobotTrajectory(JointTrajectory joint_trajectory, MultiDOFJointTrajectory multi_dof_joint_trajectory)
{
this.joint_trajectory = joint_trajectory;
this.multi_dof_joint_trajectory = multi_dof_joint_trajectory;
}
public AttachedCollisionObject(string link_name, CollisionObject @object, string[] touch_links, JointTrajectory detach_posture, double weight)
{
this.link_name = link_name;
this.@object = @object;
this.touch_links = touch_links;
this.detach_posture = detach_posture;
this.weight = weight;
}
public JointTrajectoryGoal()
{
this.trajectory = new JointTrajectory();
this.relative = 0;
}
public JointTrajectoryGoal(JointTrajectory trajectory, byte relative)
{
this.trajectory = trajectory;
this.relative = relative;
}
public RosJointTrajectoryCoder_()
{
latestJointTrajectory = new JointTrajectory();
}
