From e0d86ede11bb6057a4200be1e6a815141856b12d Mon Sep 17 00:00:00 2001 From: TaigaTakano Date: Tue, 14 Nov 2023 00:41:39 +0900 Subject: [PATCH] Support TF2 Signed-off-by: TaigaTakano --- Assets/CoRE/OutdoorsScene.unity | 31 +++++++++ Assets/CoRE/Scrips/AutoBot.cs | 113 ++++++++++++++++++++++++-------- 2 files changed, 117 insertions(+), 27 deletions(-) diff --git a/Assets/CoRE/OutdoorsScene.unity b/Assets/CoRE/OutdoorsScene.unity index 54bcf8de9..f9663a407 100644 --- a/Assets/CoRE/OutdoorsScene.unity +++ b/Assets/CoRE/OutdoorsScene.unity @@ -326,6 +326,11 @@ MonoBehaviour: panel_led_2: {fileID: 194614240} red_material: {fileID: 2100000, guid: f03c79ad2f857d9a283db70af7890d5a, type: 2} blue_material: {fileID: 2100000, guid: f6313f2e2ce1fe9c8a29e752865bc0e1, type: 2} +--- !u!1 &220149300 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 4343941266098612470, guid: 3db1ce12c7394e8c691ea54c83deb48e, type: 3} + m_PrefabInstance: {fileID: 455025083775143854} + m_PrefabAsset: {fileID: 0} --- !u!115 &273566616 MonoScript: m_ObjectHideFlags: 0 @@ -388,6 +393,11 @@ Transform: m_Father: {fileID: 0} m_RootOrder: 2 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &405101803 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 2384868700371733052, guid: 3db1ce12c7394e8c691ea54c83deb48e, type: 3} + m_PrefabInstance: {fileID: 455025083775143854} + m_PrefabAsset: {fileID: 0} --- !u!1 &412981584 GameObject: m_ObjectHideFlags: 0 @@ -690,6 +700,11 @@ Transform: m_Father: {fileID: 1568566513} m_RootOrder: 2 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &500680248 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 1225856462404870608, guid: d1dc7dc68c027c6d292183335b9c78cd, type: 3} + m_PrefabInstance: {fileID: 1225856463201321993} + m_PrefabAsset: {fileID: 0} --- !u!1 &592731387 GameObject: m_ObjectHideFlags: 0 @@ -4806,6 +4821,11 @@ Transform: m_Father: {fileID: 1498996900} m_RootOrder: 51 m_LocalEulerAnglesHint: {x: 0, y: 90, z: 0} +--- !u!1 &1781018333 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 8314345171938726905, guid: 3db1ce12c7394e8c691ea54c83deb48e, type: 3} + m_PrefabInstance: {fileID: 455025083775143854} + m_PrefabAsset: {fileID: 0} --- !u!1 &1823688464 GameObject: m_ObjectHideFlags: 0 @@ -6129,6 +6149,12 @@ MonoBehaviour: gun_target_gameobj: {fileID: 1347229477} red_ball: {fileID: 7535681071301648070, guid: 7392121f1cca0e3f9b0822d8441ae8ea, type: 3} green_ball: {fileID: 7535681071301648070, guid: 81b9aa47d403eeae480430ae3a6f0bab, type: 3} + enemy_robot_1: {fileID: 921720821} + base_target_1: {fileID: 1781018333} + base_target_2: {fileID: 220149300} + base_target_3: {fileID: 405101803} + realsense: {fileID: 500680248} + rs_color: {fileID: 2078661230} motor_noise_percent: 0.1 --- !u!1001 &2005185630 PrefabInstance: @@ -6237,6 +6263,11 @@ Transform: m_Father: {fileID: 0} m_RootOrder: 4 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!1 &2078661230 stripped +GameObject: + m_CorrespondingSourceObject: {fileID: 1225856462028142559, guid: d1dc7dc68c027c6d292183335b9c78cd, type: 3} + m_PrefabInstance: {fileID: 1225856463201321993} + m_PrefabAsset: {fileID: 0} --- !u!1 &2086521137 stripped GameObject: m_CorrespondingSourceObject: {fileID: 2665384022462445883, guid: 50b399be373994d18a4966e3617a0f81, type: 3} diff --git a/Assets/CoRE/Scrips/AutoBot.cs b/Assets/CoRE/Scrips/AutoBot.cs index fdd501fe0..5484df8d9 100644 --- a/Assets/CoRE/Scrips/AutoBot.cs +++ b/Assets/CoRE/Scrips/AutoBot.cs @@ -12,12 +12,21 @@ public class AutoBot : MonoBehaviour public GameObject red_ball; public GameObject green_ball; + public GameObject enemy_robot_1; + + public GameObject base_target_1; + public GameObject base_target_2; + public GameObject base_target_3; + + public GameObject realsense; + public GameObject rs_color; + + private ROS2UnityComponent ros2Unity; private ROS2Node ros2Node; private ISubscription control_sub; - private IPublisher robo_pose_pub; - private IPublisher gun_pose_pub; + private IPublisher tf_pub; private geometry_msgs.msg.Twist sub_msg; private double target_gun; @@ -36,6 +45,29 @@ void Update() } + geometry_msgs.msg.Transform globalObjTramsformToROS2(GameObject obj){ + var transform_rt = new geometry_msgs.msg.Transform(); + transform_rt.Translation.X = (float)(obj.transform.position.x); + transform_rt.Translation.Y = (float)(obj.transform.position.z); + transform_rt.Translation.Z = (float)(obj.transform.position.y); + transform_rt.Rotation.X = (float)(obj.transform.rotation.x); + transform_rt.Rotation.Y = (float)(obj.transform.rotation.z); + transform_rt.Rotation.Z = (float)(obj.transform.rotation.w); + transform_rt.Rotation.W = (float)(obj.transform.rotation.y); + return transform_rt; + } + geometry_msgs.msg.Transform localObjTramsformToROS2(GameObject obj){ + var transform_rt = new geometry_msgs.msg.Transform(); + transform_rt.Translation.X = (float)(obj.transform.localPosition.x); + transform_rt.Translation.Y = (float)(obj.transform.localPosition.z); + transform_rt.Translation.Z = (float)(obj.transform.localPosition.y); + transform_rt.Rotation.X = (float)(obj.transform.localRotation.x); + transform_rt.Rotation.Y = (float)(obj.transform.localRotation.z); + transform_rt.Rotation.Z = (float)(obj.transform.localRotation.w); + transform_rt.Rotation.W = (float)(obj.transform.localRotation.y); + return transform_rt; + } + void FixedUpdate() { if(ros2Unity.Ok()) @@ -48,8 +80,7 @@ void FixedUpdate() "control/cmd_vel", msg => { sub_msg = msg; }); - robo_pose_pub = ros2Node.CreatePublisher("pose/robot"); - gun_pose_pub = ros2Node.CreatePublisher("pose/gun"); + tf_pub = ros2Node.CreatePublisher("/tf"); } // Auto Robot Yaw Control @@ -98,29 +129,57 @@ void FixedUpdate() // Robot State Pub - geometry_msgs.msg.Pose msg_pose = new geometry_msgs.msg.Pose(); - msg_pose.Position.X = (float)(this.transform.position.x); - msg_pose.Position.Y = (float)(this.transform.position.z); - msg_pose.Position.Z = (float)(this.transform.position.y); - - Quaternion unityQuat = new Quaternion(this.transform.rotation.x, this.transform.rotation.y, this.transform.rotation.z, this.transform.rotation.w); - Quaternion rosQuat = Quaternion.Euler(-90, 0, 0) * unityQuat; - msg_pose.Orientation.X = (float)(rosQuat.x); - msg_pose.Orientation.Y = (float)(rosQuat.z); - msg_pose.Orientation.W = (float)(rosQuat.w); - msg_pose.Orientation.Z = (float)(rosQuat.y); - robo_pose_pub.Publish(msg_pose); - - msg_pose.Position.X = (float)(gun_gameobj.transform.localPosition.x); - msg_pose.Position.Y = (float)(gun_gameobj.transform.localPosition.z); - msg_pose.Position.Z = (float)(gun_gameobj.transform.localPosition.y); - unityQuat = new Quaternion(gun_gameobj.transform.localRotation.x, gun_gameobj.transform.localRotation.y, gun_gameobj.transform.localRotation.z, gun_gameobj.transform.localRotation.w); - rosQuat = Quaternion.Euler(-90, 0, 0) * unityQuat; - msg_pose.Orientation.X = (float)(rosQuat.x); - msg_pose.Orientation.Y = (float)(rosQuat.z); - msg_pose.Orientation.W = (float)(rosQuat.w); - msg_pose.Orientation.Z = (float)(rosQuat.y); - gun_pose_pub.Publish(msg_pose); + var pose_robot = new geometry_msgs.msg.TransformStamped(); + pose_robot.Header.Frame_id = "map"; + pose_robot.Child_frame_id = "base_link"; + pose_robot.Transform = globalObjTramsformToROS2(this.gameObject); + + var gun_robot = new geometry_msgs.msg.TransformStamped(); + gun_robot.Header.Frame_id = "base_link"; + gun_robot.Child_frame_id = "gun"; + gun_robot.Transform = localObjTramsformToROS2(gun_gameobj.gameObject); + + var pose_rs = new geometry_msgs.msg.TransformStamped(); + pose_rs.Header.Frame_id = "base_link"; + pose_rs.Child_frame_id = "camera_link"; + pose_rs.Transform = localObjTramsformToROS2(realsense.gameObject); + + var pose_rs_c = new geometry_msgs.msg.TransformStamped(); + pose_rs_c.Header.Frame_id = "camera_link"; + pose_rs_c.Child_frame_id = "camera_color_frame"; + pose_rs_c.Transform = localObjTramsformToROS2(rs_color.gameObject); + + + // demo + var pose_enemy_robot_1 = new geometry_msgs.msg.TransformStamped(); + pose_enemy_robot_1.Header.Frame_id = "map"; + pose_enemy_robot_1.Child_frame_id = "enemy_robot_1_link"; + pose_enemy_robot_1.Transform = globalObjTramsformToROS2(enemy_robot_1); + + var pose_base_target_1 = new geometry_msgs.msg.TransformStamped(); + pose_base_target_1.Header.Frame_id = "map"; + pose_base_target_1.Child_frame_id = "base_target_1_link"; + pose_base_target_1.Transform = globalObjTramsformToROS2(base_target_1); + + var pose_base_target_2 = new geometry_msgs.msg.TransformStamped(); + pose_base_target_2.Header.Frame_id = "map"; + pose_base_target_2.Child_frame_id = "base_target_2_link"; + pose_base_target_2.Transform = globalObjTramsformToROS2(base_target_2); + + var pose_base_target_3 = new geometry_msgs.msg.TransformStamped(); + pose_base_target_3.Header.Frame_id = "map"; + pose_base_target_3.Child_frame_id = "base_target_3_link"; + pose_base_target_3.Transform = globalObjTramsformToROS2(base_target_3); + + // demo end + + var tf = new tf2_msgs.msg.TFMessage(); + tf.Transforms = new geometry_msgs.msg.TransformStamped[8]{ + pose_robot,gun_robot,pose_rs,pose_rs_c, + pose_enemy_robot_1, + pose_base_target_1,pose_base_target_2,pose_base_target_3 + }; + tf_pub.Publish(tf); } } }