ROS与Unity通讯总结

ROS与Unity 3D通讯总结

参考文章:https://github.com/Unity-Technologies/Unity-Robotics-Hub

https://github.com/Unity-Technologies/ROS-TCP-Endpoint

https://github.com/Unity-Technologies/ROS-TCP-Connector

目前使用是GitHub开源的Unity-Robotics-Hub项目,其中在Unity的PC端使用ROS#已经集中

ROS-TCP-Connector中,双方通讯使用TCP协议,低延迟.

据官方faq资料显示,发送100张100 1036x1698的图片:

• ROS#和ROS Bridge Suite平均每张图片花费10秒,在收到前10条左右的信息后，速度急剧下降。

• ROS-TCP-Connector与ROS-TCP-Endpoint平均花费~0.6秒每张图像。

Unity3D运行环境

• 版本:Unity3D 2020.2.1f1

• 系统:Windows10

• 平台:PC

ROS运行环境

• 版本:ROS-Melodic
• 系统:Ubuntu18.04
• 平台:Jetson Nano3
• 建图框架:Rtabmap

目的

将Rtabmap中的Topic的/rtabmap/mapPath数据映射到Unity3D.

备注:此文章省略Rtabmap的建图过程,只要以ROS和Unity3D的通讯为主.

Unity3D端教程

下载项目文件

使用 git下载ROS-TCP-Connector到指定目录

git clone https://github.com/Unity-Technologies/ROS-TCP-Connector.git

使用Unity3D添加项目,项目路径为刚才下载的文件夹\ROS-TCP-Connector\TestRosTcpConnector文件夹

该项目实设置好的Unity项目其中已经自动关联好对应的ROS-TCP-Connector包,可以在Window->Package Manager中查看.目前最新的把版本为是0.3.0,以GitHub上的为准.

设置ROS的IP

在上面项目导入成功的情况下,在Unity的功能面部中会出现对应的Robotics功能选项,点击Robotics->ROS Setting面板

将运行ROS的IP地址添到ROS IP Address中.

编写订阅Topic的脚本

脚本内容如下:主要是订阅/rtabmap/mapPath的路径数据,并将新的路径点生产一个axisCube对象,在场景中方便观察.

备注:关于通过Unity向ROS的Topic发送数据可以参考脚本RosPublisherExample

using Unity.Robotics.ROSTCPConnector;
using UnityEngine;
using Path = RosMessageTypes.Nav.MPath;

public class UnitySubscriberMapPath : MonoBehaviour
{
    public GameObject axisCube;

    private void Start()
    {
        // 订阅/rtabmap/mapPath 必须要对应相同的数据类型Path
        ROSConnection.instance.Subscribe<Path>("/rtabmap/mapPath", ColorChange);
    }

    private void ColorChange(Path mapPath)
    {
        // Debug.Log(mapPath.header.frame_id);
        // Debug.Log(mapPath.header.stamp.nsecs);
        // Debug.Log(mapPath.poses.Length);

        var c = mapPath.poses[mapPath.poses.Length - 1];
        Debug.Log(c.pose.ToString());

        var tempObj = Instantiate(axisCube);
        tempObj.transform.parent = transform;

        var pos = c.pose.position;
        // Unity's* **(x,y,z)** *is equivalent to the ROS* **(z,-x,y)** *coordinate
        tempObj.transform.localPosition = new Vector3((float)pos.x, (float)pos.y, (float)pos.z);
        var orientation = c.pose.orientation;

        tempObj.transform.localRotation = new Quaternion((float)orientation.x, (float)orientation.y, (float)orientation.z, (float)orientation.w);
    }
}

• 在Unity场景中新建一个Example对象设置Position(0,0,0),Rotation(0,0,0)

• 将上面的UnitySubscriberMapPath脚本挂载到上面

• 将场景中的Axis对象拖住到脚本中对应的AxisCube槽中;

• Axis对象可以是一个简单的Cube,或者是一个手动制作的仿照的轴向物体.

Axis对象

ROS与Unity通讯示意图

Ros端教程

在Ubuntu你应该有一个可以正常运行的ROS环境和一个正常运行的Rtabmap服务.

此时使用rtabmap_ROS的Wiki提供的数据包.此数据包下载需要翻墙

改数据包对应的launch 可以通过命令roslaunch demo_robot_mapping.launch启动

下载ROS-TCP-Endpoint

需要将ROS-TCP-Endpoint下载到ROS的工作目录下的src文件夹下,我的ROS的工作目录为catkin_ws,这个目录替换成你自己路径.

cd catkin_ws/src/
git clone https://github.com/Unity-Technologies/ROS-TCP-Endpoint

设置ROS的IP

nano ROS-TCP-Endpoint/config/params.yaml 
// 将params.yaml中的127.0.0.1改成当前的ROS的IP地址

编写发送topic的python脚本

该脚本会直接启动TCPServer经常并订阅/rtabmap/mapPath的topic数据通过TCP发送出去.

• 在ROS-TCP-Endpoint/src文件下新建toUnityMapPath.py脚本,脚本内容如下:

#!/usr/bin/env python

import rospy

from ros_tcp_endpoint import TcpServer, RosPublisher, RosSubscriber, RosService, UnityService
from nav_msgs.msg import Path
def main():
    ros_node_name = rospy.get_param("/TCP_NODE_NAME", 'TCPServer')
    buffer_size = rospy.get_param("/TCP_BUFFER_SIZE", 1024)
    connections = rospy.get_param("/TCP_CONNECTIONS", 10)
    tcp_server = TcpServer(ros_node_name, buffer_size, connections)
    rospy.init_node(ros_node_name, anonymous=True)
    
    tcp_server.start({
        '/rtabmap/mapPath': RosSubscriber('/rtabmap/mapPath', Path, tcp_server),
    })
    
    rospy.spin()


if __name__ == "__main__":
    main()

• 编译

  cd ~/catkin_ws
  
  catkin_make
  
  source devel/setup.bash
  
  

ROS Topic 常用命令

rostopic list // 查看当前的topic
rostopic echo <TopicName> // 输出当前的Topic数据
rostopic info <TopicName> // 查看对应的发布和订阅信息
rosmsg show <MessageName> 查看message数据结构

编写toUnityMapPath.py脚本

• 查看/rtabmap/mapData的订阅者信息

  rostopic info /rtabmap/mapPath
  
  -------输出--------
  
  $ rostopic info /rtabmap/mapPath
  Type: nav_msgs/Path
  
  Publishers: 
   * /rtabmap/rtabmap (http://192.168.122.245:37497/)
  
  Subscribers: None
  
  

  当前只用本机的rtabmapviz有订阅/rtabmap/mapData

• 启动脚本

启动ROS相关进程

• 启动rosore

  roscore
  

• 新开一个命令行,启动demo_robot_mapping.launch

  roslaunch demo_robot_mapping.launch
  

• 新开一个命令行启动toUnityMapPath.py

  rosrun ROS-TCP-Endpoint toUnityMapPath.py
  

  此时TCPServer 已经订阅到/rtabmap/mapPath

• 查看/rtabmap/mapData的订阅者信息

  rostopic info /rtabmap/mapPath
  
  -------输出--------
  
  ~$ rostopic info /rtabmap/mapPath
  Type: nav_msgs/Path
  
  Publishers: 
   * /rtabmap/rtabmap (http://192.168.122.245:37497/)
  
  Subscribers: 
   * /TCPServer_17102_1619767144528 (http://192.168.122.245:33349/)
  

• 新开一个命令行播放数据包

  rosbag play --clock demo_mapping.bag
  

• 启动Unity,此时已经可以看到Unity界面中的axisCube对象生成的路径和rtabmap中路径对应上了.

附加

Ros与Unity交互 在Github有开源的通讯框架:Unity-Technologies/Unity-Robotics-Hub: Central repository for tools, tutorials, resources, and documentation for robotics simulation in Unity. (github.com)

参考其中的tutorials/ros_unity_integration 项目

rtabmap中主要通过topic 的 /rtabmap/mapPath发送当前的位置和角度数据 ,

在运行的时候可以用 命令rostopic echo /rtabmap/mapPath查看

数据格式如下:

- 
    header: 
      seq: 0
      stamp: 
        secs: 1368730248
        nsecs: 656214750
      frame_id: "map"
    pose: 
      position: 
        x: 5.20650577545
        y: -0.0206401012838
        z: 0.0
      orientation: 
        x: 0.0
        y: 0.0
        z: 0.893005369008
        w: 0.450045868986

其中header结构体参考:std_msgs/Header Documentation (ros.org)

pose结构体参考:geometry_msgs/Pose Documentation (ros.org)

传输的数据结构体参考:nav_msgs/Path Documentation (ros.org)

直接使用ros自带的消息类型 nva_msg

# python中
from nav_msgs.msg import Path

// C#中
using Path = RosMessageTypes.Nav.MPath;

mapPath节点为topic非service通过ROS订阅后再通过TCP框架推送过去

先尝试在ros中订阅并通过py打印出来 直接使用TCPServer的订阅推送功能就行

可以使用**rostopic info **查看对应的发布和订阅者点信息

rostopic info <TopicName>

ros坐标系ROS坐标系统，常见的坐标系和其含义 | 蓝鲸ROS机器人论坛 (bwbot.org)

https://github.com/Unity-Technologies/ROS-TCP-Connector/blob/main/ROSGeometry.md

Note: Going from Unity world space to ROS world space requires a conversion. Unity’s (x,y,z) is equivalent to the ROS (z,-x,y) coordinate. These conversions are provided via the [ROSGeometry component]**(https://github.com/Unity-Technologies/ROS-TCP-Connector/blob/main/ROSGeometry.md) in the ROS-TCP-Connector package.

x 轴指向机器人前方
y 轴指向机器人左方
z 轴指向机器人上方

实际使用中坐标系根据自己的需求调整

ce requires a conversion. Unity’s* (x,y,z) is equivalent to the ROS (z,-x,y) coordinate. These conversions are provided via the [ROSGeometry component]**(https://github.com/Unity-Technologies/ROS-TCP-Connector/blob/main/ROSGeometry.md) in the ROS-TCP-Connector package.

x 轴指向机器人前方
y 轴指向机器人左方
z 轴指向机器人上方

实际使用中坐标系根据自己的需求调整

• mapPath数据映射到Unity中,实现路径在Unity中可以实时更新.