Question Simple Bag Replay w/ Odometry to Make a Cube Move - Help Getting Started for ROS Noob

Hey there - I barely got this package running in Ubuntu with a ROS Container running Melodic and ran the color cube demo - it connects and the cube changes color, yay!

Now, I am so lost as to where I even begin to do what I actually need it to do. It sounds simple - I run a bag replay in my ROS container (replay is an autonomous helicopter flying around), and I want to make a unity game object subscribe to the odometry topic and move its position and rotation accordingly (to show it in a fancy unity scene). One of my coworkers had this working in ROS#, but we could no longer get it to connect, so we are using Unity Robotics and the latest version of Unity, which seems to use a totally different way to subscribe to topics.

Can anyone help guide me or explain to me in normal words how I am supposed to get this package working? I don't have msg or srv files to work with to generate C#, I have no idea where those even come in to play or where to find them. And the demo has some scripts for what looks like pose and rotation, but I have no idea how to create the subscriber for it like in the color change example or how I'd then translate that into the odometry topics being spit out.

Where does one even begin? Anything I put into the ROS connection TF topics array gives me this error:

Code (CSharp):

1. SysCommand.subscribe - Unknown message class 'tf2_msgs/TFMessage'

literally anything I put there and I have tried every conceivable combination to try and subscribe to the Odometry topic. Desperate for help...

 

I sorted it out, nav odometry is already built in so you don't have to worry about msg or srv. I replay the bag and created a script to listen to the odometry. Important to note that when running the bag you have to specify the topics with the topic flag and then put pose_estimation/ltp/odometry to get the right message here. This also has some extra math to set the stuff correctly from the pose messages.

Here it is:

Code (CSharp):

1. using UnityEngine;
2. using Unity.Robotics.ROSTCPConnector;
3. using Unity.Robotics.ROSTCPConnector.ROSGeometry;
4. using RosOdom = RosMessageTypes.Nav.OdometryMsg;
5.  
6. using System;
7. using System.Linq;
8. using System.Collections.Generic;
9. using System.Text;
10.  
11. public class RosSubscriberExample : MonoBehaviour
12. {
13.  
14.     [Header("Helicopter")]
15.     //public PhantomController helicopterController;
16.     public GameObject helicopterBody;
17.     public Vector3 offset;
18.     public Quaternion roff;
19.  
20.     private Vector3 position = Vector3.zero;
21.     private float positionVel = 0f;
22.     private Quaternion rotation = Quaternion.identity;
23.     private float rotationVel = 0f;
24.     private bool isMessageReceived = false;
25.  
26.     private float sumVels = 0f;
27.     private float countVels = 0f;
28.  
29.  
30.  
31.     public void Start()
32.     {
33.         ROSConnection.GetOrCreateInstance().Subscribe<RosOdom>("pose_estimation/ltp/odometry", OdomChange);
34.         Debug.Log("Odometry Subscriber Setup.");
35.     }
36.  
37.     private void FixedUpdate() {
38.  
39.         if(isMessageReceived){
40.  
41.             var vel = countVels > 0 ? sumVels / countVels : positionVel;
42.             vel = Mathf.Max(vel, positionVel);
43.             //Debug.Log("Found " + countVels + " entries. Average = " + vel);
44.             sumVels = countVels = 0;
45.  
46.             helicopterBody.transform.position = Vector3.LerpUnclamped(helicopterBody.transform.position, position, Time.fixedDeltaTime * vel);
47.             //helicopterBody.transform.position = position;
48.             helicopterBody.transform.rotation = Quaternion.Lerp(helicopterBody.transform.rotation, rotation, Time.fixedDeltaTime * rotationVel);
49.         }
50.      
51.     }
52.  
53.     //callback for subscription
54.     private void OdomChange(RosOdom odomMessage)
55.     {
56.         //Debug.Log("received");
57.         var p = GetPosition(odomMessage);
58.         position = new Vector3(p.y, -p.z * 1.5f, p.x);
59.         position += offset;
60.  
61.         var q = GetRotation(odomMessage);
62.         rotation = new Quaternion(q.y * roff.y, q.z * roff.z, q.x * roff.x, q.w * roff.w);
63.  
64.         positionVel = GetLinearVelocity(odomMessage);
65.         rotationVel = GetAngularVelocity(odomMessage);
66.  
67.         sumVels += positionVel;
68.         countVels++;
69.         //Debug.Log("added");
70.  
71.         isMessageReceived = true;
72.     }
73.     private void ProcessMessage()
74.     {
75.         //PublishedTransform.position = position;
76.         //PublishedTransform.rotation = rotation;
77.     }
78.  
79.     private Vector3 GetPosition(RosOdom odomMessage)
80.     {
81.         return new Vector3(
82.             (float)odomMessage.pose.pose.position.x,
83.             (float)odomMessage.pose.pose.position.y,
84.             (float)odomMessage.pose.pose.position.z);
85.     }
86.  
87.     private Quaternion GetRotation(RosOdom odomMessage)
88.     {
89.         return new Quaternion(
90.             (float)odomMessage.pose.pose.orientation.x,
91.             (float)odomMessage.pose.pose.orientation.y,
92.             (float)odomMessage.pose.pose.orientation.z,
93.             (float)odomMessage.pose.pose.orientation.w);
94.     }
95.  
96.     private float GetLinearVelocity(RosOdom odomMessage)
97.     {
98.         var R = new Vector4(
99.             (float)odomMessage.pose.pose.orientation.x,
100.             (float)odomMessage.pose.pose.orientation.y,
101.             (float)odomMessage.pose.pose.orientation.z,
102.             (float)odomMessage.pose.pose.orientation.w);
103.         var vel_body = new Vector4(
104.             (float)odomMessage.twist.twist.linear.x,
105.             (float)odomMessage.twist.twist.linear.y,
106.             (float)odomMessage.twist.twist.linear.z,
107.             1.0f);
108.         var v = Mathf.Abs(Vector4.Dot(R, vel_body));
109.         //Debug.Log("Linear velocity: " + v);
110.         return v;
111.     }
112.  
113.     private float GetAngularVelocity(RosOdom odomMessage)
114.     {
115.         var R = new Vector4(
116.             (float)odomMessage.pose.pose.orientation.x,
117.             (float)odomMessage.pose.pose.orientation.y,
118.             (float)odomMessage.pose.pose.orientation.z,
119.             (float)odomMessage.pose.pose.orientation.w);
120.         var vel_body = new Vector4(
121.             (float)odomMessage.twist.twist.angular.x,
122.             (float)odomMessage.twist.twist.angular.y,
123.             (float)odomMessage.twist.twist.angular.z,
124.             1.0f);
125.         var v = Mathf.Abs(Vector4.Dot(R, vel_body));
126.         //Debug.Log("Angular velocity: " + v);
127.         return v;
128.     }
129. }

 

Mr. Shane, I've been looking for this for a long time, and now that I read your message, it answers a lot of my questions. But I still have some questions to ask. If I want to obtain ros' pose topic, endowed with objects, may I ask what I should do?