/// <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)); }