浏览代码

Updating Ros-Unity integration tutorial

/devin-main-fix
LaurieCheers 4 年前
当前提交
bd944260
共有 25 个文件被更改,包括 482 次插入3 次删除
  1. 2
      tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs
  2. 3
      tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs
  3. 1
      tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs
  4. 8
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo.meta
  5. 8
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg.meta
  6. 89
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/PosRot.cs
  7. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/PosRot.cs.meta
  8. 68
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/UnityColor.cs
  9. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/UnityColor.cs.meta
  10. 8
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv.meta
  11. 46
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceRequest.cs
  12. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceRequest.cs.meta
  13. 46
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceResponse.cs
  14. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceResponse.cs.meta
  15. 51
      tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosPublisherExample.cs
  16. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosPublisherExample.cs.meta
  17. 60
      tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosServiceExample.cs
  18. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosServiceExample.cs.meta
  19. 18
      tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosSubscriberExample.cs
  20. 11
      tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosSubscriberExample.cs.meta

2
tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs


using RosMessageTypes.RoboticsDemo;
using UnityEngine;
using Random = UnityEngine.Random;
using Unity.Robotics.ROSTCPConnector;
/// <summary>
///

3
tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs


using System;
using Debug = UnityEngine.Debug;
using Unity.Robotics.ROSTCPConnector;
public class RosServiceExample : MonoBehaviour
{

1
tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs


using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.RoboticsDemo.UnityColor;
public class RosSubscriberExample : MonoBehaviour

8
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo.meta


fileFormatVersion: 2
guid: f8d5f220ed0f08d4d8f4fb2de4738f0d
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

8
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg.meta


fileFormatVersion: 2
guid: ebc2e4980541514439a08d616e51be72
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

89
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/PosRot.cs


//Do not edit! This file was generated by Unity-ROS MessageGeneration.
using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;
namespace RosMessageTypes.RoboticsDemo
{
public class PosRot : Message
{
public const string RosMessageName = "robotics_demo/PosRot";
public float pos_x;
public float pos_y;
public float pos_z;
public float rot_x;
public float rot_y;
public float rot_z;
public float rot_w;
public PosRot()
{
this.pos_x = 0.0f;
this.pos_y = 0.0f;
this.pos_z = 0.0f;
this.rot_x = 0.0f;
this.rot_y = 0.0f;
this.rot_z = 0.0f;
this.rot_w = 0.0f;
}
public PosRot(float pos_x, float pos_y, float pos_z, float rot_x, float rot_y, float rot_z, float rot_w)
{
this.pos_x = pos_x;
this.pos_y = pos_y;
this.pos_z = pos_z;
this.rot_x = rot_x;
this.rot_y = rot_y;
this.rot_z = rot_z;
this.rot_w = rot_w;
}
public override List<byte[]> SerializationStatements()
{
var listOfSerializations = new List<byte[]>();
listOfSerializations.Add(BitConverter.GetBytes(this.pos_x));
listOfSerializations.Add(BitConverter.GetBytes(this.pos_y));
listOfSerializations.Add(BitConverter.GetBytes(this.pos_z));
listOfSerializations.Add(BitConverter.GetBytes(this.rot_x));
listOfSerializations.Add(BitConverter.GetBytes(this.rot_y));
listOfSerializations.Add(BitConverter.GetBytes(this.rot_z));
listOfSerializations.Add(BitConverter.GetBytes(this.rot_w));
return listOfSerializations;
}
public override int Deserialize(byte[] data, int offset)
{
this.pos_x = BitConverter.ToSingle(data, offset);
offset += 4;
this.pos_y = BitConverter.ToSingle(data, offset);
offset += 4;
this.pos_z = BitConverter.ToSingle(data, offset);
offset += 4;
this.rot_x = BitConverter.ToSingle(data, offset);
offset += 4;
this.rot_y = BitConverter.ToSingle(data, offset);
offset += 4;
this.rot_z = BitConverter.ToSingle(data, offset);
offset += 4;
this.rot_w = BitConverter.ToSingle(data, offset);
offset += 4;
return offset;
}
public override string ToString()
{
return "PosRot: " +
"\npos_x: " + pos_x.ToString() +
"\npos_y: " + pos_y.ToString() +
"\npos_z: " + pos_z.ToString() +
"\nrot_x: " + rot_x.ToString() +
"\nrot_y: " + rot_y.ToString() +
"\nrot_z: " + rot_z.ToString() +
"\nrot_w: " + rot_w.ToString();
}
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/PosRot.cs.meta


fileFormatVersion: 2
guid: d158eeb9d2ab2b34389562ecc86d3fba
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

68
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/UnityColor.cs


//Do not edit! This file was generated by Unity-ROS MessageGeneration.
using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;
namespace RosMessageTypes.RoboticsDemo
{
public class UnityColor : Message
{
public const string RosMessageName = "robotics_demo/UnityColor";
public int r;
public int g;
public int b;
public int a;
public UnityColor()
{
this.r = 0;
this.g = 0;
this.b = 0;
this.a = 0;
}
public UnityColor(int r, int g, int b, int a)
{
this.r = r;
this.g = g;
this.b = b;
this.a = a;
}
public override List<byte[]> SerializationStatements()
{
var listOfSerializations = new List<byte[]>();
listOfSerializations.Add(BitConverter.GetBytes(this.r));
listOfSerializations.Add(BitConverter.GetBytes(this.g));
listOfSerializations.Add(BitConverter.GetBytes(this.b));
listOfSerializations.Add(BitConverter.GetBytes(this.a));
return listOfSerializations;
}
public override int Deserialize(byte[] data, int offset)
{
this.r = BitConverter.ToInt32(data, offset);
offset += 4;
this.g = BitConverter.ToInt32(data, offset);
offset += 4;
this.b = BitConverter.ToInt32(data, offset);
offset += 4;
this.a = BitConverter.ToInt32(data, offset);
offset += 4;
return offset;
}
public override string ToString()
{
return "UnityColor: " +
"\nr: " + r.ToString() +
"\ng: " + g.ToString() +
"\nb: " + b.ToString() +
"\na: " + a.ToString();
}
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/msg/UnityColor.cs.meta


fileFormatVersion: 2
guid: 9d138bc4b22c060409fae4fd5384cb36
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

8
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv.meta


fileFormatVersion: 2
guid: 282662e82b04a5a458c8e4d259bac322
folderAsset: yes
DefaultImporter:
externalObjects: {}
userData:
assetBundleName:
assetBundleVariant:

46
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceRequest.cs


//Do not edit! This file was generated by Unity-ROS MessageGeneration.
using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;
namespace RosMessageTypes.RoboticsDemo
{
public class PositionServiceRequest : Message
{
public const string RosMessageName = "robotics_demo/PositionService";
public PosRot input;
public PositionServiceRequest()
{
this.input = new PosRot();
}
public PositionServiceRequest(PosRot input)
{
this.input = input;
}
public override List<byte[]> SerializationStatements()
{
var listOfSerializations = new List<byte[]>();
listOfSerializations.AddRange(input.SerializationStatements());
return listOfSerializations;
}
public override int Deserialize(byte[] data, int offset)
{
offset = this.input.Deserialize(data, offset);
return offset;
}
public override string ToString()
{
return "PositionServiceRequest: " +
"\ninput: " + input.ToString();
}
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceRequest.cs.meta


fileFormatVersion: 2
guid: d858f45a03c13c642b29b3ff4a9c56df
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

46
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceResponse.cs


//Do not edit! This file was generated by Unity-ROS MessageGeneration.
using System;
using System.Linq;
using System.Collections.Generic;
using System.Text;
using Unity.Robotics.ROSTCPConnector.MessageGeneration;
namespace RosMessageTypes.RoboticsDemo
{
public class PositionServiceResponse : Message
{
public const string RosMessageName = "robotics_demo/PositionService";
public PosRot output;
public PositionServiceResponse()
{
this.output = new PosRot();
}
public PositionServiceResponse(PosRot output)
{
this.output = output;
}
public override List<byte[]> SerializationStatements()
{
var listOfSerializations = new List<byte[]>();
listOfSerializations.AddRange(output.SerializationStatements());
return listOfSerializations;
}
public override int Deserialize(byte[] data, int offset)
{
offset = this.output.Deserialize(data, offset);
return offset;
}
public override string ToString()
{
return "PositionServiceResponse: " +
"\noutput: " + output.ToString();
}
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/RosMessages/RoboticsDemo/srv/PositionServiceResponse.cs.meta


fileFormatVersion: 2
guid: 795b7ba01d3e5a3449231021c7888b14
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

51
tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosPublisherExample.cs


using RosMessageTypes.RoboticsDemo;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
/// <summary>
///
/// </summary>
public class RosPublisherExample : MonoBehaviour
{
ROSConnection ros;
public string topicName = "pos_rot";
// The game object
public GameObject cube;
// Publish the cube's position and rotation every N seconds
public float publishMessageFrequency = 0.5f;
// Used to determine how much time has elapsed since the last message was published
private float timeElapsed;
void Start()
{
// start the ROS connection
ros = ROSConnection.instance;
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
cube.transform.rotation = Random.rotation;
PosRot cubePos = new PosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,
cube.transform.rotation.x,
cube.transform.rotation.y,
cube.transform.rotation.z,
cube.transform.rotation.w
);
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, cubePos);
timeElapsed = 0;
}
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosPublisherExample.cs.meta


fileFormatVersion: 2
guid: 2def3f665688fd04d8edd131dd3f76dd
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

60
tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosServiceExample.cs


using RosMessageTypes.RoboticsDemo;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
public class RosServiceExample : MonoBehaviour
{
ROSConnection ros;
public string serviceName = "pos_srv";
public GameObject cube;
// Cube movement conditions
public float delta = 1.0f;
public float speed = 2.0f;
private Vector3 destination;
float awaitingResponseUntilTimestamp = -1;
void Start()
{
ros = ROSConnection.instance;
destination = cube.transform.position;
}
private void Update()
{
// Move our position a step closer to the target.
float step = speed * Time.deltaTime; // calculate distance to move
cube.transform.position = Vector3.MoveTowards(cube.transform.position, destination, step);
if (Vector3.Distance(cube.transform.position, destination) < delta && Time.time > awaitingResponseUntilTimestamp)
{
Debug.Log("Destination reached.");
PosRot cubePos = new PosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,
cube.transform.rotation.x,
cube.transform.rotation.y,
cube.transform.rotation.z,
cube.transform.rotation.w
);
PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos);
// Send message to ROS and return the response
ros.SendServiceMessage<PositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
awaitingResponseUntilTimestamp = Time.time+1.0f; // don't send again for 1 second, or until we receive a response
}
}
void Callback_Destination(PositionServiceResponse response)
{
awaitingResponseUntilTimestamp = -1;
destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z);
Debug.Log("New Destination: " + destination);
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosServiceExample.cs.meta


fileFormatVersion: 2
guid: 21d643cc61a51a24ba6bb08ad9d63ad9
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

18
tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosSubscriberExample.cs


using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.RoboticsDemo.UnityColor;
public class RosSubscriberExample : MonoBehaviour
{
public GameObject cube;
void Start()
{
ROSConnection.instance.Subscribe<RosColor>("color", ColorChange);
}
void ColorChange(RosColor colorMessage)
{
cube.GetComponent<Renderer>().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a);
}
}

11
tutorials/pick_and_place/PickAndPlaceProject/Assets/Scripts/RosSubscriberExample.cs.meta


fileFormatVersion: 2
guid: a84c3317220ed43429cdfc91fdc91e95
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:
正在加载...
取消
保存