private void ServiceCallHandler(GetCartesianPathResponse message)
{
float[][] posArray = message.solution.joint_trajectory.points
.Select(point => point.positions
.Select(pos => (float)pos)
.ToArray())
.ToArray();
float[][] velArray = message.solution.joint_trajectory.points
.Select(point => point.velocities
.Select(vel => (float)vel)
.ToArray())
.ToArray();
float[][] accArray = message.solution.joint_trajectory.points
.Select(point => point.accelerations
.Select(acc => (float)acc)
.ToArray())
.ToArray();
float[] timeArray = message.solution.joint_trajectory.points
.Select(point => point.time_from_start.secs + point.time_from_start.nsecs / 1e9f)
.ToArray();
float fractionOfTrajectory = (float)message.fraction * 100;
trajectoryDict.Add(instanceID, new Dictionary <int, object>
{
{ 0, fractionOfTrajectory },
{ 1, timeArray },
{ 2, posArray },
{ 3, velArray },
{ 4, accArray },
});
OnServiceReceived.Set();
}
/// <summary> Setter constructor. </summary>
public GetCartesianPath(GetCartesianPathRequest request)
{
Request = request;
Response = new GetCartesianPathResponse();
}
/// <summary> Empty constructor. </summary>
public GetCartesianPath()
{
Request = new GetCartesianPathRequest();
Response = new GetCartesianPathResponse();
}
