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();
        }
Example #2
0
 /// <summary> Setter constructor. </summary>
 public GetCartesianPath(GetCartesianPathRequest request)
 {
     Request  = request;
     Response = new GetCartesianPathResponse();
 }
Example #3
0
 /// <summary> Empty constructor. </summary>
 public GetCartesianPath()
 {
     Request  = new GetCartesianPathRequest();
     Response = new GetCartesianPathResponse();
 }