Skip to content

Instantly share code, notes, and snippets.

@Akashleena
Created August 26, 2021 07:05
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save Akashleena/dc24c86d42cf83d0993771666ffc38b8 to your computer and use it in GitHub Desktop.
Save Akashleena/dc24c86d42cf83d0993771666ffc38b8 to your computer and use it in GitHub Desktop.
publish pos_rot message of a cube from Unity3D to ROS side but convert the co-ordinate frame and then publish the message so that Gazebo and Unity simulation looks exactly identical
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosMessageTypes.UnityRoboticsDemo;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Unity.Robotics.UrdfImporter;
using RosMessageTypes.Geometry;
/// <summary>
///
/// </summary>
public class RosPublisherExample : MonoBehaviour
{
ROSConnection ros;
public string topicName = "pos_rot";
// The game object
public GameObject cube;
// Publish the cube's position and rotation every N seconds
public float publishMessageFrequency = 0.5f;
// Used to determine how much time has elapsed since the last message was published
private float timeElapsed;
void Start()
{
// start the ROS connection
ros = ROSConnection.instance;
ros.RegisterPublisher<PosRotMsg>(topicName);
ros.RegisterPublisher<PosRotMsg>("new_pos_rot");
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
cube.transform.rotation = Random.rotation;
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
);
Vector3<FLU> rosPos = cube.transform.position.To<FLU>();
Quaternion<FLU> rosPos2=cube.transform.rotation.To<FLU>();
PosRotMsg msg = new PosRotMsg();
msg.pos_x=rosPos.x;
msg.pos_y=rosPos.y;
msg.pos_z=rosPos.z;
msg.rot_x = rosPos2.x;
msg.rot_y = rosPos2.y;
msg.rot_z = rosPos2.z;
msg.rot_w = rosPos2.w;
ros.Send("new_pos_rot", msg);
timeElapsed = 0;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment