0. 简介
在我们上一章讲的对于Unity与ROS之间的通信，Toolbox等比较基础的指令。下面我们将结合代码来介绍一下如何导入URDF文件，并通过键盘完成Unity的控制。
1. URDF模型创建
- 首先先创建并编辑URDF文件，并命名为toio_style.urdf
<?xml version="1.0"?>
<robot name="toio_style">
<!--车身-->
<link name="base_link">
<visual>
<geometry>
<box size="0.3 0.3 0.23" />
</geometry>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0" />
</material>
</visual>
<collision>
<geometry>
<box size="0.3 0.3 0.23" />
</geometry>
</collision>
<inertial>
<mass value="1.0" />
<inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0" />
</inertial>
</link>
<!--右车轮-->
<link name="right_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.035" />
</geometry>
<material name="gray">
<color rgba="0.2 0.2 0.2 1" />
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.035" />
</geometry>
</collision>
<inertial>
<mass value="0.1" />
<inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0" />
</inertial>
</link>
<joint name="right_wheel_joint" type="continuous">
<axis xyz="0 0 1" />
<parent link="base_link" />
<child link="right_wheel" />
<origin rpy="-1.5708 0 0" xyz="0.0 -0.125 -.09" />
</joint>
<!--左车轮-->
<link name="left_wheel">
<visual>
<geometry>
<cylinder length="0.05" radius="0.035" />
</geometry>
<material name="gray" />
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.035" />
</geometry>
</collision>
<inertial>
<mass value="0.1" />
<inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0" />
</inertial>
</link>
<joint name="left_wheel_joint" type="continuous">
<axis xyz="0 0 1" />
<parent link="base_link" />
<child link="left_wheel" />
<origin rpy="-1.5708 0 0" xyz="0.0 0.125 -.09" />
</joint>
</robot>
2. ROS处准备
- 启动Docker映像，并指定10000和5005端口打开。因为Unity和ROS之间的通信需要端口号10000和5005。
docker run -v ~/ros2_ws:/home/ubuntu/colcon_ws:cached -p 6080:80 -p 10000:10000 -p 5005:5005 --shm-size=1024m tiryoh/ros2-desktop-vnc:galactic
- 安装“ROS-TCP-Endpoint”软件包。并使用“main-ros2”分支中的ROS-TCP-Endpoint包
cd ~/colcon_ws/src
git clone -b main-ros2 https://github.com/Unity-Technologies/ROS-TCP-Endpoint
- 然后创建工作空间
cd ~/colcon_ws
colcon build
source ~/colcon_ws/install/setup.bash
3. Unity处准备
Unity方面的准备步骤基本和第一章提到的类似，即：
-
在Unity菜单“Window→Package Manager”中打开“Package Manager”。
-
在“Package Manager”中选择“+→Add Package from Git URL”，输入以下URL，按下“Add”按钮。并输入以下链接加载ROS-TCP-Connector插件
https://github.com/Unity-Technologies/ROS-TCP-Connector.git?path=/com.unity.robotics.ros-tcp-connector
- Package Manager”中选择“+→Add Package from Git URL”，输入以下URL，按下“Add”按钮。并输入以下链接加载URDF Importer插件
https://github.com/Unity-Technologies/URDF-Importer.git?path=/com.unity.robotics.urdf-importer
4. URDF模型导入
将URDF模型导入Unity的场景中。
-
在Unity的Assets中配置“toio_style.urdf”。
-
在Project窗口中，右键点击“toio_style.urdf”，选择“Import Robot Select form URDF file”
- Import URDF
-
在Hierarchy窗口用Plane制作地板
6. ROS2控制Unity
为了操作URDF模型，在URDF模型中添加Robotics-Nav2-SLAM-Example中的AGVController。这样Unity就可以接受来自/cmd_vel主话题的信息，用于控制Unity当中的机器人了。
-
在Hierarchy窗口中选择URDF模型(toio_style)，在Inspector窗口中取消Controller组件的检查。
-
下载Robotics-Nav2-SLAM-Example包并将AGVController脚本文件放到Unity的Assets中。
- 在Unity中将导入的URDF模型(toio_style)中添加AGVController脚本，即在加载脚本后，将脚本的Wheel1变量添加toio_style的left_wheel的TF信息, Wheel2中添加toio_style的right_wheel的TF信息。
·Wheel 1:车轮1
·Wheel 2:车轮2
·Mode:模式(ROS / Keyboard)
·Max Linear Speed:最大直行速度
·Max Rotation Speed:最大转速
·Wheel Radius:车轮半径
·Track Width:轨道宽度
·Force Limit:力量的最大值
·Damping:衰减
·ROS Timeout:一段时间内没有ROS消息时停止
- 在AGVController”的Mode中指定Keyboard，按下Play按钮，用键盘确认机器人的动作
- 然后打开终端，指定ROS的IP和端口，执行ROS2的ROS-TCP-Endpoint指令
ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=0.0.0.0
在“AGVController”的Mode中指定ROS，按下Play按钮。如果成功连接到ROS2，左上角的“ROS IP”箭头就会变成蓝色。
在ROS侧输入以下命令，发布/cmd_vel主题。从而完成对Unity中的URDF模型运行
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear: {x: 0.5}, angular: {z: 0.5}}'
具体代码如下
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.Geometry;
using Unity.Robotics.UrdfImporter.Control;
namespace RosSharp.Control
{
public enum ControlMode { Keyboard, ROS};
public class AGVController : MonoBehaviour
{
public GameObject wheel1;
public GameObject wheel2;
public ControlMode mode = ControlMode.ROS;
private ArticulationBody wA1;
private ArticulationBody wA2;
public float maxLinearSpeed = 2; // m/s
public float maxRotationalSpeed = 1;//
public float wheelRadius = 0.033f; //meters
public float trackWidth = 0.288f; // meters Distance between tyres
public float forceLimit = 10;
public float damping = 10;
public float ROSTimeout = 0.5f;
private float lastCmdReceived = 0f;
ROSConnection ros;
private RotationDirection direction;
private float rosLinear = 0f;
private float rosAngular = 0f;
void Start()
{
wA1 = wheel1.GetComponent<ArticulationBody>();
wA2 = wheel2.GetComponent<ArticulationBody>();
SetParameters(wA1);
SetParameters(wA2);
ros = ROSConnection.GetOrCreateInstance();
ros.Subscribe<TwistMsg>("cmd_vel", ReceiveROSCmd);
}
void ReceiveROSCmd(TwistMsg cmdVel)
{
rosLinear = (float)cmdVel.linear.x;
rosAngular = (float)cmdVel.angular.z;
lastCmdReceived = Time.time;
}
void FixedUpdate()
{
if (mode == ControlMode.Keyboard)
{
KeyBoardUpdate();
}
else if (mode == ControlMode.ROS)
{
ROSUpdate();
}
}
private void SetParameters(ArticulationBody joint)
{
ArticulationDrive drive = joint.xDrive;
drive.forceLimit = forceLimit;
drive.damping = damping;
joint.xDrive = drive;
}
private void SetSpeed(ArticulationBody joint, float wheelSpeed = float.NaN)
{
ArticulationDrive drive = joint.xDrive;
if (float.IsNaN(wheelSpeed))
{
drive.targetVelocity = ((2 * maxLinearSpeed) / wheelRadius) * Mathf.Rad2Deg * (int)direction;
}
else
{
drive.targetVelocity = wheelSpeed;
}
joint.xDrive = drive;
}
private void KeyBoardUpdate()
{
float moveDirection = Input.GetAxis("Vertical");
float inputSpeed;
float inputRotationSpeed;
if (moveDirection > 0)
{
inputSpeed = maxLinearSpeed;
}
else if (moveDirection < 0)
{
inputSpeed = maxLinearSpeed * -1;
}
else
{
inputSpeed = 0;
}
float turnDirction = Input.GetAxis("Horizontal");
if (turnDirction > 0)
{
inputRotationSpeed = maxRotationalSpeed;
}
else if (turnDirction < 0)
{
inputRotationSpeed = maxRotationalSpeed * -1;
}
else
{
inputRotationSpeed = 0;
}
RobotInput(inputSpeed, inputRotationSpeed);
}
private void ROSUpdate()
{
if (Time.time - lastCmdReceived > ROSTimeout)
{
rosLinear = 0f;
rosAngular = 0f;
}
RobotInput(rosLinear, -rosAngular);
}
private void RobotInput(float speed, float rotSpeed) // m/s and rad/s
{
if (speed > maxLinearSpeed)
{
speed = maxLinearSpeed;
}
if (rotSpeed > maxRotationalSpeed)
{
rotSpeed = maxRotationalSpeed;
}
float wheel1Rotation = (speed / wheelRadius);
float wheel2Rotation = wheel1Rotation;
float wheelSpeedDiff = ((rotSpeed * trackWidth) / wheelRadius);
if (rotSpeed != 0)
{
wheel1Rotation = (wheel1Rotation + (wheelSpeedDiff / 1)) * Mathf.Rad2Deg;
wheel2Rotation = (wheel2Rotation - (wheelSpeedDiff / 1)) * Mathf.Rad2Deg;
}
else
{
wheel1Rotation *= Mathf.Rad2Deg;
wheel2Rotation *= Mathf.Rad2Deg;
}
SetSpeed(wA1, wheel1Rotation);
SetSpeed(wA2, wheel2Rotation);
}
}
}
7. 从Unity发送TF坐标系
-
除了上面所说的以外，为了要将Unity的信息有效的传输给ROS2需要以下几个脚本
・Clock.cs
・ROSTransformTreePublisher.cs
・TimeStamp.cs
・TransformExtensions.cs
・TransformTreeNode.cs -
在URDF模型(toio_style)中添加空GameObject，并指定名称为ROSPublishers
- 在ROSPublishers中添加了输出的Topic为ROSTransformTreePublisher类型，并在Robot Game Object中，设置为URDF模型的base_link
- 然后再次启动ROS2脚本
ros2 run ros_tcp_endpoint default_server_endpoint --ros-args -p ROS_IP:=0.0.0.0
- 这样就可以订阅当前的状态了
ros2 topic echo /tf
transforms:
- header:
stamp:
sec: 35
nanosec: 864012140
frame_id: map
child_frame_id: base_link
transform:
translation:
x: 0.0036436456721276045
y: -3.143894957702287e-07
z: 0.12480859458446503
rotation:
x: -6.655823767687252e-07
y: -3.76763296117133e-07
z: 9.396488167112693e-06
w: -1.0
- header:
stamp:
sec: 35
nanosec: 864012140
frame_id: base_link
child_frame_id: left_wheel
transform:
translation:
x: 2.148539124391391e-09
y: 0.1250000149011612
z: -0.09999997913837433
rotation:
x: 0.7061359286308289
y: -0.037066373974084854
z: -0.03706619143486023
w: -0.7061333060264587
- header:
stamp:
sec: 35
nanosec: 864012140
frame_id: base_link
child_frame_id: right_wheel
transform:
translation:
x: 2.103767826611147e-09
y: -0.1250000149011612
z: -0.09999997913837433
rotation:
x: 0.7061364650726318
y: -0.03705677390098572
z: -0.03705659508705139
w: -0.7061338424682617
- header:
stamp:
sec: 35
nanosec: 864012140
frame_id: base_link
child_frame_id: back_wheel
transform:
translation:
x: -0.09999999403953552
y: 4.364437700132839e-09
z: -0.10000000894069672
rotation:
x: 0.706138551235199
y: -0.03701629117131233
z: -0.037016112357378006
w: -0.7061359286308289
- header:
stamp:
sec: 35
nanosec: 864012140
frame_id: base_link
child_frame_id: front_wheel
transform:
translation:
x: 0.10000003129243851
y: 4.607613846019376e-09
z: -0.09999999403953552
rotation:
x: 0.7061392068862915
y: -0.03700389340519905
z: -0.03700371831655502
w: -0.7061365842819214
:
评论（0）
您还未登录，请登录后发表或查看评论