/// <summary>
/// Stores Link information as a Ros message so that it will be send the next time when the user initializes to the cage
/// </summary>
/// <param name="info">the Link info.</param>
public static void StoreLinkInformation(float[] info)
{
if (info.Length % linkInfoLength != 0)
{
Debug.LogWarning("Shape mismatch: Received array length " + info.Length + " not dividable by " +
linkInfoLength);
}
int numLinks = info.Length / linkInfoLength;
linkInfo = new LinkInformation[numLinks];
for (int i = 0; i < numLinks; i++)
{
int o = i * linkInfoLength;
int id = (int)(info[o]);
string linkName = linkNames[(int)(info[o + 1])];
RosSharp.RosBridgeClient.MessageTypes.Geometry.Vector3 dimensions =
new RosSharp.RosBridgeClient.MessageTypes.Geometry.Vector3(info[o + 2], info[o + 3], info[o + 4]);
Point point = new Point(info[o + 5], info[o + 6], info[o + 7]);
RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion quaternion =
new RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion(info[o + 8], info[o + 9], info[o + 10],
info[o + 11]);
Pose init_pose = new Pose(point, quaternion);
linkInfo[i] = new LinkInformation(id, linkName, dimensions, init_pose);
}
}
protected override void ReceiveMessage(RosSharp.RosBridgeClient.MessageTypes.Gazebo.ModelStates message)
{
int index;
modelNamesFromGazebo = message.name;
for (int i = 0; i < message.name.Length; i++)
{
string name = message.name[i];
RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose pose = message.pose[i];
// Gazebo is in meters, we think in millimeters
/*
* pose.position.x = pose.position.x * 1000.0f;
* pose.position.y = pose.position.y * 1000.0f;
* pose.position.z = pose.position.z * 1000.0f;
* pose.position.x = pose.position.x - differenceGazeboUnity.x;
* pose.position.y = pose.position.y - differenceGazeboUnity.y;
* pose.position.z = pose.position.z - differenceGazeboUnity.z;
*/
Twist twist = message.twist[i];
if (name.Contains("ADO"))
{
index = ModelNames.IndexOf(name);
//Debug.Log($"{name} x: {pose.position.x} y: {pose.position.x} z: {pose.position.z} ");
if (index != -1)
{
if (ModelStateWriters.Count > index)
{
ModelStateWriter msw = ModelStateWriters[index];
if (msw != null)
{
msw.Write(pose, twist);
}
}
}
else
{
RosSharp.RosBridgeClient.MessageTypes.Gazebo.ModelState modelstate = new RosSharp.RosBridgeClient.MessageTypes.Gazebo.ModelState(name, pose, twist, "");
if (modelsToGen.Exists(model => { return(model.model_name.Equals(modelstate.model_name)); }))
{
}
else
{
if (ModelNames.Contains(modelstate.model_name))
{
}
else
{
modelsToGen.Add(modelstate);
}
}
}
}
}
}
public void Write(RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose pose, RosSharp.RosBridgeClient.MessageTypes.Geometry.Twist twist)
{
UnityEngine.Vector3 newPosition;
newROSTransformPos = new UnityEngine.Vector3((float)pose.position.x, (float)pose.position.y, (float)pose.position.z);
newROSTransformPos = newROSTransformPos * 1000f;
newPosition = coordinateTargetSelector.transformVectorRos2UnityLocal(newROSTransformPos);
newTargetTransformPos = newPosition;
}
private void InitializeMessage()
{
message = new MessageTypes.Geometry.PoseArray
{
header = new MessageTypes.Std.Header()
{
frame_id = FrameId
}
};
local_pose = new MessageTypes.Geometry.Pose();
message.poses = new MessageTypes.Geometry.Pose[50];
waypoint_array = new GameObject[50];
}
private void UpdateMessage()
{
if (trigger_clicked.GetStateUp(SteamVR_Input_Sources.Any))
{
waypoint_position = waypoint_plotter_tracker.position; //Tracking the hand controller
onplay_waypoint = Instantiate(waypoint_prefab, waypoint_position, Quaternion.identity); //Instantiate waypoints in the field of view
waypoint_array[click_count] = onplay_waypoint;
//Conversion of the coordinates to be sent to the ros
/*local_pose.position.x = onplay_waypoint.transform.position.z - 3;
* local_pose.position.y = -onplay_waypoint.transform.position.x;
* local_pose.position.z = onplay_waypoint.transform.position.y;
*
* message.poses[click_count] = local_pose; //storing the pose in the pose array
*
* local_pose = new MessageTypes.Geometry.Pose(); //Clear the local pose */
click_count = click_count + 1;
}
if (publish_clicked.GetStateUp(SteamVR_Input_Sources.Any))
{
Array.Resize(ref waypoint_array, click_count);
message.poses = new MessageTypes.Geometry.Pose[click_count];
for (int i = 0; i < click_count; i++)
{
//Conversion of the coordinates to be sent to the ros
local_pose.position.x = waypoint_array[i].transform.position.z - 3;
local_pose.position.y = -waypoint_array[i].transform.position.x;
local_pose.position.z = waypoint_array[i].transform.position.y;
message.poses[i] = local_pose;
local_pose = new MessageTypes.Geometry.Pose();
}
publish_flag = 1;
//Array.Resize(ref message.poses, click_count + 1);
}
if (publish_flag == 1)
{
Publish(message);
}
}
public static RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose ToRosPose(this IPose pose)
{
(var pos, var rot) = (pose.ToPositionVector(), pose.ToRotationQuaternion());
pos = RosSharp.TransformExtensions.Unity2Ros(pos);
rot = RosSharp.TransformExtensions.Unity2Ros(rot);
RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose p = new RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose();
p.position = new RosSharp.RosBridgeClient.MessageTypes.Geometry.Point()
{
x = pos.x,
y = pos.y,
z = pos.z
};
p.orientation = new RosSharp.RosBridgeClient.MessageTypes.Geometry.Quaternion()
{
x = rot.x,
y = rot.y,
z = rot.z,
w = rot.w
};
return(p);
}
private ModelStateWriter generateNewObject(RosSharp.RosBridgeClient.MessageTypes.Gazebo.ModelState modelstate)
{
RosSharp.RosBridgeClient.MessageTypes.Geometry.Pose pose = modelstate.pose;
Twist twist = modelstate.twist;
string name = modelstate.model_name;
GameObject ado = Instantiate(generatedCubePrefab);
generatedObjects.Add(ado);
ado.name = name;
ado.transform.SetParent(genCubeParentTransform, true);
ModelStateWriter modelStateWriter = ado.AddComponent <ModelStateWriter>();
CollisionTester collisionTester = ado.AddComponent <CollisionTester>();
collisionTester.currentExecuter = this.currentExecutor;
collisionTester.sucker = this.LoaderSucker;
suctionCupStatusSubscriber.collisionTesters.Add(collisionTester);
BoxCollider boxCollider = ado.GetComponent <BoxCollider>();
boxCollider.isTrigger = true;
foreach (UnityEngine.Transform child in ado.transform)
{
if (child.name.Contains("ROSCoords"))
{
child.localPosition = new UnityEngine.Vector3((float)pose.position.x, (float)pose.position.y, (float)pose.position.z);
child.localPosition = child.localPosition * 1000f;
ado.transform.localPosition = coordinateTargetSelector.transformVectorRos2UnityLocal(child.localPosition);
modelStateWriter.rosTransform = child;
}
}
modelStateWriter.targetTransform = ado.transform;
modelStateWriter.updateTransform = true;
modelStateWriter.coordinateTargetSelector = this.coordinateTargetSelector;
coordinateTargetSelector.addOnClickHandlerAndBoundingBoxes(name);
return(modelStateWriter);
}
private void InitializeMessage()
{
header = new MessageTypes.Std.Header(0, new MessageTypes.Std.Time(), "map");
pose = new MessageTypes.Geometry.Pose();
message = new MessageTypes.Geometry.PoseStamped(header, pose);
}
/// <summary>
/// Converts a Transform and given name to an EFPose
/// </summary>
/// <param name="ef_transform">The transform of which the data should be read out.</param>
/// <param name="hand_id">The name of the transform/effector.</param>
/// <returns></returns>
private EFPose transformToEFPose(Transform ef_transform, string hand_id)
{
Pose pose = CageInterface.TransformToPose(ef_transform);
return(new EFPose(hand_id, pose));
}
