Skip to content

Commit

Permalink
Support TF2
Browse files Browse the repository at this point in the history 
Signed-off-by: TaigaTakano <[email protected]>
  • Loading branch information
@TakanoTaiga
TakanoTaiga committed Nov 13, 2023
1 parent 39bab9a commit e0d86ed
Show file tree
Hide file tree
Showing 2 changed files with 117 additions and 27 deletions.
31 changes: 31 additions & 0 deletions Assets/CoRE/OutdoorsScene.unity
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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}
Expand Down
113 changes: 86 additions & 27 deletions Assets/CoRE/Scrips/AutoBot.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<geometry_msgs.msg.Twist> control_sub;
private IPublisher<geometry_msgs.msg.Pose> robo_pose_pub;
private IPublisher<geometry_msgs.msg.Pose> gun_pose_pub;
private IPublisher<tf2_msgs.msg.TFMessage> tf_pub;
private geometry_msgs.msg.Twist sub_msg;
private double target_gun;

Expand All @@ -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())
Expand All @@ -48,8 +80,7 @@ void FixedUpdate()
"control/cmd_vel", msg => {
sub_msg = msg;
});
robo_pose_pub = ros2Node.CreatePublisher<geometry_msgs.msg.Pose>("pose/robot");
gun_pose_pub = ros2Node.CreatePublisher<geometry_msgs.msg.Pose>("pose/gun");
tf_pub = ros2Node.CreatePublisher<tf2_msgs.msg.TFMessage>("/tf");
}

// Auto Robot Yaw Control
Expand Down Expand Up @@ -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);
}
}
}
Expand Down

0 comments on commit e0d86ed

Please sign in to comment.