概要

  • 这篇博客将介绍:
    • 创建一个简单的 Unity 场景,该场景在 Unity 中运行服务,该服务接受具有游戏对象名称的请求,并使用游戏对象在 ROS 坐标系中的姿势(位置和方向)进行响应。
    • 创建一个简单的 Unity 场景,该场景调用具有游戏对象位置和旋转的外部ROS 服务,以接收将游戏对象移动到的新位置。
    • 在学习这篇博客之前,需要完成 【Unity-Robotic-Hub 入门教程(一)】Unity 与 ROS 1/2 联动之环境配置

UnityService

创建 Unity 服务

  • 创建新的 C# 脚本并将其命名RosUnityServiceExample.cs
  • 将以下代码粘贴到RosUnityServiceExample.cs ,这个脚本在存储库中的相对路径为 tutorials/ros_unity_integration/unity_scripts
using RosMessageTypes.UnityRoboticsDemo;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;

/// <summary>
/// 实现UnityService的示例演示,该UnityService从另一个ROS节点接收请求消息并发送回响应
/// </summary>
public class RosUnityServiceExample : MonoBehaviour
{
    [SerializeField]
    string m_ServiceName = "obj_pose_srv";

    void Start()
    {
        // 向ROS注册服务
        ROSConnection.GetOrCreateInstance().ImplementService<ObjectPoseServiceRequest, ObjectPoseServiceResponse>(m_ServiceName, GetObjectPose);
    }

    /// <summary>
    ///  回掉以响应请求
    /// </summary>
    /// <param name="request">service request containing the object name</param>
    /// <returns>包含对象姿势的服务响应(如果未找到对象,则为0)</returns>
    private ObjectPoseServiceResponse GetObjectPose(ObjectPoseServiceRequest request)
    {
        // 处理服务请求
        Debug.Log("Received request for object: " + request.object_name);

        // 准备回应
        ObjectPoseServiceResponse objectPoseResponse = new ObjectPoseServiceResponse();
        // 查找具有请求名称的游戏对象
        GameObject gameObject = GameObject.Find(request.object_name);
        if (gameObject)
        {
            // 用从Unity坐标转换为ROS坐标系的对象姿势Fill-in响应
            objectPoseResponse.object_pose.position = gameObject.transform.position.To<FLU>();
            objectPoseResponse.object_pose.orientation = gameObject.transform.rotation.To<FLU>();
        }

        return objectPoseResponse;
    }
}
  • 创建一个空的游戏对象并命名它为 UnityService
  • 将脚本 RosUnityServiceExample 附加到游戏对象 UnityService
  • 在编辑器中按播放应该开始作为 ROS 节点运行,等待接受 ObjectPose 请求。一旦与ROS建立了连接,将在ROS终端上打印一条类似于 Connection from 172.17.0.1 的消息。

启动客户端

  • 为了测试新服务是否正常工作,使用内置的 ROS 服务命令调用它。
  • 在 ROS1 中,在 ROS 终端中执行以下命令:
rosservice call /obj_pose_srv Cube
  • 在 Unity 控制台中,应该会看到日志消息 Received request for object: Cube ,在终端中,它将报告对象的位置,如下所示:
object_pose:
 position:
   x: 0.0
   y: -1.0
   z: 0.20000000298023224
 orientation:
   x: 0.0
   y: -0.0
   z: 0.0
   w: -1.0
  • 如果使用的是 ROS2,则命令为:
ros2 service call obj_pose_srv unity_robotics_demo_msgs/ObjectPoseService "{object_name: Cube}"
  • 输出将如下所示:
requester: making request: unity_robotics_demo_msgs.srv.ObjectPoseService_Request(object_name='Cube')
response:
unity_robotics_demo_msgs.srv.ObjectPoseService_Response(object_pose=geometry_msgs.msg.Pose(position=geometry_msgs.msg.Point(x=0.0, y=-0.0, z=0.0), orientation=geometry_msgs.msg.Quaternion(x=-0.558996319770813, y=-0.3232670724391937, z=-0.6114855408668518, w=-0.4572822153568268)))

调用服务

启动位置服务

  • 在本教程中,需要一个 ros 服务供 Unity 调用。在新的终端窗口中,导航到您的 ROS 工作区。
  • 在 ROS1 中,执行以下命令:
source devel/setup.bash
rosrun unity_robotics_demo position_service.py
  • 在 ROS2 中,改为运行:
source install/setup.bash
ros2 run unity_robotics_demo position_service

创建 Unity 服务调用方

  • 创建脚本并命名RosServiceCallExample.cs
  • 将以下代码粘贴到RosServiceCallExample.cs ,(或者,可以将脚本文件从 tutorials/ros_unity_integration/unity_scripts 中拖到 Unity 中)。
using RosMessageTypes.UnityRoboticsDemo;
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;

public class RosServiceCallExample : 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.GetOrCreateInstance();
        ros.RegisterRosService<PositionServiceRequest, PositionServiceResponse>(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);
    }
}
  • 创建一个空的游戏对象并命名它为 RosService
  • 将脚本 RosServiceExample 附加到游戏对象 RosService ,将立方体游戏对象拖动到其参数 cube 上。
  • 在编辑器中按播放应该开始与脚本通信,作为 ROS 节点运行,使立方体移动到场景中的随机位置。