您最多选择25个主题
主题必须以中文或者字母或数字开头,可以包含连字符 (-),并且长度不得超过35个字符
61 行
2.1 KiB
61 行
2.1 KiB
using RosMessageTypes.UnityRoboticsDemo;
|
|
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;
|
|
ros.RegisterRosService<PositionServiceRequest>(serviceName);
|
|
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.");
|
|
|
|
PosRotMsg cubePos = new PosRotMsg(
|
|
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);
|
|
}
|
|
}
|