private void SendStopMessage()
{
Twist message = new Twist();
message.linear = stopLinear;
message.angular = stopAngular;
cmdPublisher.Publish(message);
}
private void SendPointCloud(TimeSpan currentTime, List <Matrix4x4> transforms, List <Vector3[]> vertices)
{
int oneAxisValueByteSize = 4; // 4(float32)
int oneVertexByteSize = 3 * oneAxisValueByteSize;
int numberOfVertices = vertices.Sum(group => group.Length);
byte[] data = new byte[numberOfVertices * oneVertexByteSize];
int ngroups = transforms.Count;
int copiedUntilNow = 0;
for (int i = 0; i < ngroups; i++)
{
Matrix4x4 currentTransfrom = transforms[i];
Vector3[] currentVertices = vertices[i];
int subNumberOfVertices = currentVertices.Length;
for (int j = 0; j < subNumberOfVertices; j++)
{
Vector3 vertex = currentVertices[j];
Vector3 transformedVertex = currentTransfrom.MultiplyPoint3x4(vertex);
transformedVertex = HololensRobotController.Utilities.CoordinateTransformations.ConvertPositionUnity2ROS(transformedVertex);
Buffer.BlockCopy(BitConverter.GetBytes(transformedVertex.x), 0, data, copiedUntilNow + j * oneVertexByteSize, oneAxisValueByteSize);
Buffer.BlockCopy(BitConverter.GetBytes(transformedVertex.y), 0, data, copiedUntilNow + j * oneVertexByteSize + oneAxisValueByteSize, oneAxisValueByteSize);
Buffer.BlockCopy(BitConverter.GetBytes(transformedVertex.z), 0, data, copiedUntilNow + j * oneVertexByteSize + 2 * oneAxisValueByteSize, oneAxisValueByteSize);
}
copiedUntilNow = copiedUntilNow + subNumberOfVertices * oneVertexByteSize;
}
// pulish the message
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
RosSharp.RosBridgeClient.Messages.Sensor.PointCloud2 message = new RosSharp.RosBridgeClient.Messages.Sensor.PointCloud2();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = frameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
message.height = 1;
message.width = numberOfVertices;
message.fields = pointFields;
message.is_bigendian = false;
message.is_dense = true;
message.point_step = oneVertexByteSize;
message.row_step = message.width * message.point_step;
message.data = data;
publisher.Publish(message);
}
private void PublishFrame(object sender, HololensRobotController.WindowsSensors.FrameEventArgs e)
{
int[] structuredTime = HololensRobotController.Utilities.Timer.GetSecondsNanosecondsStructure(e.TimeSpan);
RosSharp.RosBridgeClient.Messages.Sensor.Image image = new RosSharp.RosBridgeClient.Messages.Sensor.Image();
image.header.frame_id = Config.Mono;
image.header.stamp.secs = structuredTime[0];
image.header.stamp.nsecs = structuredTime[1];
image.header.seq = frameNo++;
image.data = e.Buffer;
image.encoding = e.Encoding;
image.is_bigendian = 0;
image.height = (int)e.Height;
image.width = (int)e.Width;
image.step = (int)e.Step;
publisher.Publish(image);
System.Diagnostics.Debug.WriteLine("Mono camera sent frame-" + image.header.seq + " at time " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString());
}
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 + ")");
}
private void SendPose(TimeSpan currentTime, Quaternion currentRotation, Vector3 currentPosition)
{
int[] structuredTime = Timer.GetSecondsNanosecondsStructure(currentTime);
RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped message = new RosSharp.RosBridgeClient.Messages.Geometry.PoseStamped();
message.header.frame_id = Config.HololensWorldFrame;
message.header.seq = frameIdx++;
message.header.stamp.secs = structuredTime[0];
message.header.stamp.nsecs = structuredTime[1];
CoordinateTransformations.ConvertPoseUnity2ROS(ref currentPosition, ref currentRotation);
message.pose.orientation.x = currentRotation.x;
message.pose.orientation.y = currentRotation.y;
message.pose.orientation.z = currentRotation.z;
message.pose.orientation.w = currentRotation.w;
message.pose.position.x = currentPosition.x;
message.pose.position.y = currentPosition.y;
message.pose.position.z = currentPosition.z;
publisher.Publish(message);
System.Diagnostics.Debug.WriteLine("POSE at time: " + ((double)structuredTime[0] + ((double)structuredTime[1]) / 1e9).ToString() +
" --- (" + currentPosition.x + ", " + currentPosition.y + ", " + currentPosition.z + ")");
}
