Skip to content
This repository was archived by the owner on Jul 30, 2021. It is now read-only.

Commit 42fa45b

Browse files
esm-tmoriesm-tmori
committed
add pdu accessor
1 parent 6747d0c commit 42fa45b

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

48 files changed

+1695
-17
lines changed

‎Assets/Scripts/Hakoniwa/PluggableAsset/Assets/Robot/TB3/MotorController.cs

Lines changed: 7 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
using Hakoniwa.PluggableAsset.Communication.Pdu;
2+
using Hakoniwa.PluggableAsset.Communication.Pdu.Accessor;
23
using System;
34
using System.Collections;
45
using System.Collections.Generic;
@@ -14,7 +15,7 @@ public class MotorController
1415
private float[] moving_distance = new float[2]; // 0: R, 1: L
1516
private int motor_power = 500;
1617
private float motor_interval_distance = 16.0f; // 16cm
17-
private IPduReader pdu_reader;
18+
private TwistAccessor pdu_motor_control_accessor;
1819

1920
internal Motor GetRightMotor()
2021
{
@@ -28,7 +29,7 @@ internal Motor GetLeftMotor()
2829
public void Initialize(GameObject root, Transform transform, ITB3Parts parts, IPduReader pdu_reader)
2930
{
3031
GameObject obj;
31-
this.pdu_reader = pdu_reader;
32+
this.pdu_motor_control_accessor = new TwistAccessor(pdu_reader.GetReadOps().Ref(null));
3233

3334
for (int i = 0; i < 2; i++)
3435
{
@@ -63,8 +64,10 @@ public void DoActuation()
6364
double target_velocity;
6465
double target_rotation_angle_rate;
6566

66-
target_velocity = this.pdu_reader.GetReadOps().Ref("linear").GetDataFloat64("x");
67-
target_rotation_angle_rate = this.pdu_reader.GetReadOps().Ref("angular").GetDataFloat64("z");
67+
//target_velocity = this.pdu_reader.GetReadOps().Ref("linear").GetDataFloat64("x");
68+
//target_rotation_angle_rate = this.pdu_reader.GetReadOps().Ref("angular").GetDataFloat64("z");
69+
target_velocity = this.pdu_motor_control_accessor.linear.x;
70+
target_rotation_angle_rate = this.pdu_motor_control_accessor.angular.z;
6871

6972
//Debug.Log("target_velocity=" + target_velocity);
7073
//Debug.Log("target_rotation_angle_rate=" + target_rotation_angle_rate);

‎Assets/Scripts/Hakoniwa/PluggableAsset/Assets/Robot/TB3/RobotController.cs

Lines changed: 34 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
using Assets.Scripts.Hakoniwa.PluggableAsset.Assets.Robot.TB3;
1212
using Assets.Scripts.Hakoniwa.PluggableAsset.Assets.Robot;
1313
using Hakoniwa.Core.Utils;
14+
using Hakoniwa.PluggableAsset.Communication.Pdu.Accessor;
1415

1516
namespace Hakoniwa.PluggableAsset.Assets.Robot.TB3
1617
{
@@ -23,7 +24,9 @@ public class RobotController : MonoBehaviour, IInsideAssetController
2324
private PduIoConnector pdu_io;
2425
private IPduWriter pdu_laser_scan;
2526
private IPduWriter pdu_imu;
27+
private ImuAccessor pdu_imu_accessor;
2628
private IPduWriter pdu_odometry;
29+
private OdometryAccessor pdu_odometry_accessor;
2730
private IPduWriter pdu_tf;
2831
private IPduWriter pdu_joint_state;
2932
private IPduReader pdu_motor_control;
@@ -174,27 +177,42 @@ private void CalcOdometry()
174177
*/
175178
//header
176179
TimeStamp.Set(this.pdu_odometry.GetWriteOps().Ref(null));
177-
this.pdu_odometry.GetWriteOps().Ref("header").SetData("frame_id", "/odom");
180+
//this.pdu_odometry.GetWriteOps().Ref("header").SetData("frame_id", "/odom");
181+
this.pdu_odometry_accessor.header.frame_id = "/odom";
178182

179183
//child_frame_id
180-
this.pdu_odometry.GetWriteOps().SetData("child_frame_id", "/base_footprint");
184+
//this.pdu_odometry.GetWriteOps().SetData("child_frame_id", "/base_footprint");
185+
this.pdu_odometry_accessor.child_frame_id = "/base_footprint";
186+
181187
//pose.pose.position
182-
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("x", (double)current_pos.x);
183-
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("y", (double)current_pos.y);
184-
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("z", (double)current_pos.z);
188+
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("x", (double)current_pos.x);
189+
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("y", (double)current_pos.y);
190+
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("z", (double)current_pos.z);
191+
this.pdu_odometry_accessor.pose.pose.position.x = current_pos.x;
192+
this.pdu_odometry_accessor.pose.pose.position.y = current_pos.y;
193+
this.pdu_odometry_accessor.pose.pose.position.z = current_pos.z;
185194

186195
//pose.pose.orientation
187-
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").SetData("orientation",
188-
this.pdu_imu.GetReadOps().Ref("orientation"));
196+
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").SetData("orientation",
197+
// this.pdu_imu.GetReadOps().Ref("orientation"));
198+
this.pdu_odometry_accessor.pose.pose.orientation = this.pdu_imu_accessor.orientation;
189199

190200
//twist.twist.linear
191-
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("x", (double)delta_pos.x / Time.fixedDeltaTime);
192-
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("y", (double)delta_pos.y / Time.fixedDeltaTime);
193-
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("z", (double)delta_pos.z / Time.fixedDeltaTime);
201+
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("x", (double)delta_pos.x / Time.fixedDeltaTime);
202+
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("y", (double)delta_pos.y / Time.fixedDeltaTime);
203+
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("z", (double)delta_pos.z / Time.fixedDeltaTime);
204+
this.pdu_odometry_accessor.twist.twist.linear.x = delta_pos.x / Time.fixedDeltaTime;
205+
this.pdu_odometry_accessor.twist.twist.linear.y = delta_pos.y / Time.fixedDeltaTime;
206+
this.pdu_odometry_accessor.twist.twist.linear.z = delta_pos.z / Time.fixedDeltaTime;
207+
208+
194209
//twist.twist.angular
195-
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("x", (double)delta_angle.x / Time.fixedDeltaTime);
196-
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("y", (double)delta_angle.y / Time.fixedDeltaTime);
197-
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("z", (double)delta_angle.z / Time.fixedDeltaTime);
210+
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("x", (double)delta_angle.x / Time.fixedDeltaTime);
211+
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("y", (double)delta_angle.y / Time.fixedDeltaTime);
212+
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("z", (double)delta_angle.z / Time.fixedDeltaTime);
213+
this.pdu_odometry_accessor.twist.twist.angular.x = delta_angle.x / Time.fixedDeltaTime;
214+
this.pdu_odometry_accessor.twist.twist.angular.y = delta_angle.y / Time.fixedDeltaTime;
215+
this.pdu_odometry_accessor.twist.twist.angular.z = delta_angle.z / Time.fixedDeltaTime;
198216
}
199217

200218
public void DoActuation()
@@ -250,11 +268,14 @@ private void InitSensor()
250268
{
251269
throw new ArgumentException("can not found Imu pdu:" + this.GetName() + "_imuPdu");
252270
}
271+
this.pdu_imu_accessor = new ImuAccessor(this.pdu_imu.GetReadOps().Ref(null));
253272
this.pdu_odometry = this.pdu_io.GetWriter(this.GetName() + "_odomPdu");
254273
if (this.pdu_odometry == null)
255274
{
256275
throw new ArgumentException("can not found Imu pdu:" + this.GetName() + "_odomPdu");
257276
}
277+
this.pdu_odometry_accessor = new OdometryAccessor(this.pdu_odometry.GetReadOps().Ref(null));
278+
258279
this.pdu_tf = this.pdu_io.GetWriter(this.GetName() + "_tfPdu");
259280
if (this.pdu_tf == null)
260281
{

‎Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/Accessor/Ev3PduActuatorAccessor.cs

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
using Hakoniwa.PluggableAsset.Communication.Pdu;
2+
using System;
3+
using System.Collections.Generic;
4+
using System.Linq;
5+
using System.Text;
6+
using System.Threading.Tasks;
7+
8+
namespace Hakoniwa.PluggableAsset.Communication.Pdu.Accessor
9+
{
10+
class Ev3PduActuatorAccessor
11+
{
12+
private Pdu pdu;
13+
private Ev3PduActuatorHeaderAccessor pdu_head_accessor;
14+
private Ev3PduMotorAccessor[] pdu_motors_array_accessor;
15+
16+
public Ev3PduActuatorAccessor(Pdu pdu)
17+
{
18+
this.pdu = pdu;
19+
this.pdu_head_accessor = new Ev3PduActuatorHeaderAccessor(pdu.Ref("head"));
20+
var pdu_motors_array = pdu.Refs("motors");
21+
for (int i = 0; i < pdu_motors_array.Length; i++)
22+
{
23+
this.pdu_motors_array_accessor[i] = new Ev3PduMotorAccessor(pdu_motors_array[i]);
24+
}
25+
}
26+
public Ev3PduActuatorHeaderAccessor head
27+
{
28+
set
29+
{
30+
pdu_head_accessor = value;
31+
}
32+
get
33+
{
34+
return pdu_head_accessor;
35+
}
36+
}
37+
public Byte[] leds
38+
{
39+
get
40+
{
41+
return pdu.GetDataUInt8Array("leds");
42+
}
43+
}
44+
public Ev3PduMotorAccessor[] motors
45+
{
46+
get
47+
{
48+
return pdu_motors_array_accessor;
49+
}
50+
}
51+
public UInt32 gyro_reset
52+
{
53+
set
54+
{
55+
pdu.SetData("gyro_reset", value);
56+
}
57+
get
58+
{
59+
return pdu.GetDataUInt32("gyro_reset");
60+
}
61+
}
62+
}
63+
}

‎Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/Accessor/Ev3PduActuatorAccessor.cs.meta

Lines changed: 11 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/Accessor/Ev3PduActuatorHeaderAccessor.cs

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
using Hakoniwa.PluggableAsset.Communication.Pdu;
2+
using System;
3+
using System.Collections.Generic;
4+
using System.Linq;
5+
using System.Text;
6+
using System.Threading.Tasks;
7+
8+
namespace Hakoniwa.PluggableAsset.Communication.Pdu.Accessor
9+
{
10+
class Ev3PduActuatorHeaderAccessor
11+
{
12+
private Pdu pdu;
13+
14+
public Ev3PduActuatorHeaderAccessor(Pdu pdu)
15+
{
16+
this.pdu = pdu;
17+
}
18+
public string name
19+
{
20+
set
21+
{
22+
pdu.SetData("name", value);
23+
}
24+
get
25+
{
26+
return pdu.GetDataString("name");
27+
}
28+
}
29+
public UInt32 version
30+
{
31+
set
32+
{
33+
pdu.SetData("version", value);
34+
}
35+
get
36+
{
37+
return pdu.GetDataUInt32("version");
38+
}
39+
}
40+
public Int64 asset_time
41+
{
42+
set
43+
{
44+
pdu.SetData("asset_time", value);
45+
}
46+
get
47+
{
48+
return pdu.GetDataInt64("asset_time");
49+
}
50+
}
51+
public UInt32 ext_off
52+
{
53+
set
54+
{
55+
pdu.SetData("ext_off", value);
56+
}
57+
get
58+
{
59+
return pdu.GetDataUInt32("ext_off");
60+
}
61+
}
62+
public UInt32 ext_size
63+
{
64+
set
65+
{
66+
pdu.SetData("ext_size", value);
67+
}
68+
get
69+
{
70+
return pdu.GetDataUInt32("ext_size");
71+
}
72+
}
73+
}
74+
}

‎Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/Accessor/Ev3PduActuatorHeaderAccessor.cs.meta

Lines changed: 11 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/Accessor/Ev3PduColorSensorAccessor.cs

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
using Hakoniwa.PluggableAsset.Communication.Pdu;
2+
using System;
3+
using System.Collections.Generic;
4+
using System.Linq;
5+
using System.Text;
6+
using System.Threading.Tasks;
7+
8+
namespace Hakoniwa.PluggableAsset.Communication.Pdu.Accessor
9+
{
10+
class Ev3PduColorSensorAccessor
11+
{
12+
private Pdu pdu;
13+
14+
public Ev3PduColorSensorAccessor(Pdu pdu)
15+
{
16+
this.pdu = pdu;
17+
}
18+
public UInt32 color
19+
{
20+
set
21+
{
22+
pdu.SetData("color", value);
23+
}
24+
get
25+
{
26+
return pdu.GetDataUInt32("color");
27+
}
28+
}
29+
public UInt32 reflect
30+
{
31+
set
32+
{
33+
pdu.SetData("reflect", value);
34+
}
35+
get
36+
{
37+
return pdu.GetDataUInt32("reflect");
38+
}
39+
}
40+
public UInt32 rgb_r
41+
{
42+
set
43+
{
44+
pdu.SetData("rgb_r", value);
45+
}
46+
get
47+
{
48+
return pdu.GetDataUInt32("rgb_r");
49+
}
50+
}
51+
public UInt32 rgb_g
52+
{
53+
set
54+
{
55+
pdu.SetData("rgb_g", value);
56+
}
57+
get
58+
{
59+
return pdu.GetDataUInt32("rgb_g");
60+
}
61+
}
62+
public UInt32 rgb_b
63+
{
64+
set
65+
{
66+
pdu.SetData("rgb_b", value);
67+
}
68+
get
69+
{
70+
return pdu.GetDataUInt32("rgb_b");
71+
}
72+
}
73+
}
74+
}

‎Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/Accessor/Ev3PduColorSensorAccessor.cs.meta

Lines changed: 11 additions & 0 deletions
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

0 commit comments

Comments
 (0)