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

Commit

Permalink
add pdu accessor
Browse files Browse the repository at this point in the history
  • Loading branch information
esm-tmori committed Jul 29, 2021
1 parent 6747d0c commit 42fa45b
Show file tree
Hide file tree
Showing 48 changed files with 1,695 additions and 17 deletions.
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
using Hakoniwa.PluggableAsset.Communication.Pdu;
using Hakoniwa.PluggableAsset.Communication.Pdu.Accessor;
using System;
using System.Collections;
using System.Collections.Generic;
Expand All @@ -14,7 +15,7 @@ public class MotorController
private float[] moving_distance = new float[2]; // 0: R, 1: L
private int motor_power = 500;
private float motor_interval_distance = 16.0f; // 16cm
private IPduReader pdu_reader;
private TwistAccessor pdu_motor_control_accessor;

internal Motor GetRightMotor()
{
Expand All @@ -28,7 +29,7 @@ internal Motor GetLeftMotor()
public void Initialize(GameObject root, Transform transform, ITB3Parts parts, IPduReader pdu_reader)
{
GameObject obj;
this.pdu_reader = pdu_reader;
this.pdu_motor_control_accessor = new TwistAccessor(pdu_reader.GetReadOps().Ref(null));

for (int i = 0; i < 2; i++)
{
Expand Down Expand Up @@ -63,8 +64,10 @@ public void DoActuation()
double target_velocity;
double target_rotation_angle_rate;

target_velocity = this.pdu_reader.GetReadOps().Ref("linear").GetDataFloat64("x");
target_rotation_angle_rate = this.pdu_reader.GetReadOps().Ref("angular").GetDataFloat64("z");
//target_velocity = this.pdu_reader.GetReadOps().Ref("linear").GetDataFloat64("x");
//target_rotation_angle_rate = this.pdu_reader.GetReadOps().Ref("angular").GetDataFloat64("z");
target_velocity = this.pdu_motor_control_accessor.linear.x;
target_rotation_angle_rate = this.pdu_motor_control_accessor.angular.z;

//Debug.Log("target_velocity=" + target_velocity);
//Debug.Log("target_rotation_angle_rate=" + target_rotation_angle_rate);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
using Assets.Scripts.Hakoniwa.PluggableAsset.Assets.Robot.TB3;
using Assets.Scripts.Hakoniwa.PluggableAsset.Assets.Robot;
using Hakoniwa.Core.Utils;
using Hakoniwa.PluggableAsset.Communication.Pdu.Accessor;

namespace Hakoniwa.PluggableAsset.Assets.Robot.TB3
{
Expand All @@ -23,7 +24,9 @@ public class RobotController : MonoBehaviour, IInsideAssetController
private PduIoConnector pdu_io;
private IPduWriter pdu_laser_scan;
private IPduWriter pdu_imu;
private ImuAccessor pdu_imu_accessor;
private IPduWriter pdu_odometry;
private OdometryAccessor pdu_odometry_accessor;
private IPduWriter pdu_tf;
private IPduWriter pdu_joint_state;
private IPduReader pdu_motor_control;
Expand Down Expand Up @@ -174,27 +177,42 @@ private void CalcOdometry()
*/
//header
TimeStamp.Set(this.pdu_odometry.GetWriteOps().Ref(null));
this.pdu_odometry.GetWriteOps().Ref("header").SetData("frame_id", "/odom");
//this.pdu_odometry.GetWriteOps().Ref("header").SetData("frame_id", "/odom");
this.pdu_odometry_accessor.header.frame_id = "/odom";

//child_frame_id
this.pdu_odometry.GetWriteOps().SetData("child_frame_id", "/base_footprint");
//this.pdu_odometry.GetWriteOps().SetData("child_frame_id", "/base_footprint");
this.pdu_odometry_accessor.child_frame_id = "/base_footprint";

//pose.pose.position
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("x", (double)current_pos.x);
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("y", (double)current_pos.y);
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("z", (double)current_pos.z);
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("x", (double)current_pos.x);
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("y", (double)current_pos.y);
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").Ref("position").SetData("z", (double)current_pos.z);
this.pdu_odometry_accessor.pose.pose.position.x = current_pos.x;
this.pdu_odometry_accessor.pose.pose.position.y = current_pos.y;
this.pdu_odometry_accessor.pose.pose.position.z = current_pos.z;

//pose.pose.orientation
this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").SetData("orientation",
this.pdu_imu.GetReadOps().Ref("orientation"));
//this.pdu_odometry.GetWriteOps().Ref("pose").Ref("pose").SetData("orientation",
// this.pdu_imu.GetReadOps().Ref("orientation"));
this.pdu_odometry_accessor.pose.pose.orientation = this.pdu_imu_accessor.orientation;

//twist.twist.linear
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("x", (double)delta_pos.x / Time.fixedDeltaTime);
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("y", (double)delta_pos.y / Time.fixedDeltaTime);
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("z", (double)delta_pos.z / Time.fixedDeltaTime);
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("x", (double)delta_pos.x / Time.fixedDeltaTime);
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("y", (double)delta_pos.y / Time.fixedDeltaTime);
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("linear").SetData("z", (double)delta_pos.z / Time.fixedDeltaTime);
this.pdu_odometry_accessor.twist.twist.linear.x = delta_pos.x / Time.fixedDeltaTime;
this.pdu_odometry_accessor.twist.twist.linear.y = delta_pos.y / Time.fixedDeltaTime;
this.pdu_odometry_accessor.twist.twist.linear.z = delta_pos.z / Time.fixedDeltaTime;


//twist.twist.angular
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("x", (double)delta_angle.x / Time.fixedDeltaTime);
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("y", (double)delta_angle.y / Time.fixedDeltaTime);
this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("z", (double)delta_angle.z / Time.fixedDeltaTime);
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("x", (double)delta_angle.x / Time.fixedDeltaTime);
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("y", (double)delta_angle.y / Time.fixedDeltaTime);
//this.pdu_odometry.GetWriteOps().Ref("twist").Ref("twist").Ref("angular").SetData("z", (double)delta_angle.z / Time.fixedDeltaTime);
this.pdu_odometry_accessor.twist.twist.angular.x = delta_angle.x / Time.fixedDeltaTime;
this.pdu_odometry_accessor.twist.twist.angular.y = delta_angle.y / Time.fixedDeltaTime;
this.pdu_odometry_accessor.twist.twist.angular.z = delta_angle.z / Time.fixedDeltaTime;
}

public void DoActuation()
Expand Down Expand Up @@ -250,11 +268,14 @@ private void InitSensor()
{
throw new ArgumentException("can not found Imu pdu:" + this.GetName() + "_imuPdu");
}
this.pdu_imu_accessor = new ImuAccessor(this.pdu_imu.GetReadOps().Ref(null));
this.pdu_odometry = this.pdu_io.GetWriter(this.GetName() + "_odomPdu");
if (this.pdu_odometry == null)
{
throw new ArgumentException("can not found Imu pdu:" + this.GetName() + "_odomPdu");
}
this.pdu_odometry_accessor = new OdometryAccessor(this.pdu_odometry.GetReadOps().Ref(null));

this.pdu_tf = this.pdu_io.GetWriter(this.GetName() + "_tfPdu");
if (this.pdu_tf == null)
{
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,63 @@
using Hakoniwa.PluggableAsset.Communication.Pdu;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Hakoniwa.PluggableAsset.Communication.Pdu.Accessor
{
class Ev3PduActuatorAccessor
{
private Pdu pdu;
private Ev3PduActuatorHeaderAccessor pdu_head_accessor;
private Ev3PduMotorAccessor[] pdu_motors_array_accessor;

public Ev3PduActuatorAccessor(Pdu pdu)
{
this.pdu = pdu;
this.pdu_head_accessor = new Ev3PduActuatorHeaderAccessor(pdu.Ref("head"));
var pdu_motors_array = pdu.Refs("motors");
for (int i = 0; i < pdu_motors_array.Length; i++)
{
this.pdu_motors_array_accessor[i] = new Ev3PduMotorAccessor(pdu_motors_array[i]);
}
}
public Ev3PduActuatorHeaderAccessor head
{
set
{
pdu_head_accessor = value;
}
get
{
return pdu_head_accessor;
}
}
public Byte[] leds
{
get
{
return pdu.GetDataUInt8Array("leds");
}
}
public Ev3PduMotorAccessor[] motors
{
get
{
return pdu_motors_array_accessor;
}
}
public UInt32 gyro_reset
{
set
{
pdu.SetData("gyro_reset", value);
}
get
{
return pdu.GetDataUInt32("gyro_reset");
}
}
}
}

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
using Hakoniwa.PluggableAsset.Communication.Pdu;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Hakoniwa.PluggableAsset.Communication.Pdu.Accessor
{
class Ev3PduActuatorHeaderAccessor
{
private Pdu pdu;

public Ev3PduActuatorHeaderAccessor(Pdu pdu)
{
this.pdu = pdu;
}
public string name
{
set
{
pdu.SetData("name", value);
}
get
{
return pdu.GetDataString("name");
}
}
public UInt32 version
{
set
{
pdu.SetData("version", value);
}
get
{
return pdu.GetDataUInt32("version");
}
}
public Int64 asset_time
{
set
{
pdu.SetData("asset_time", value);
}
get
{
return pdu.GetDataInt64("asset_time");
}
}
public UInt32 ext_off
{
set
{
pdu.SetData("ext_off", value);
}
get
{
return pdu.GetDataUInt32("ext_off");
}
}
public UInt32 ext_size
{
set
{
pdu.SetData("ext_size", value);
}
get
{
return pdu.GetDataUInt32("ext_size");
}
}
}
}

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
using Hakoniwa.PluggableAsset.Communication.Pdu;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Hakoniwa.PluggableAsset.Communication.Pdu.Accessor
{
class Ev3PduColorSensorAccessor
{
private Pdu pdu;

public Ev3PduColorSensorAccessor(Pdu pdu)
{
this.pdu = pdu;
}
public UInt32 color
{
set
{
pdu.SetData("color", value);
}
get
{
return pdu.GetDataUInt32("color");
}
}
public UInt32 reflect
{
set
{
pdu.SetData("reflect", value);
}
get
{
return pdu.GetDataUInt32("reflect");
}
}
public UInt32 rgb_r
{
set
{
pdu.SetData("rgb_r", value);
}
get
{
return pdu.GetDataUInt32("rgb_r");
}
}
public UInt32 rgb_g
{
set
{
pdu.SetData("rgb_g", value);
}
get
{
return pdu.GetDataUInt32("rgb_g");
}
}
public UInt32 rgb_b
{
set
{
pdu.SetData("rgb_b", value);
}
get
{
return pdu.GetDataUInt32("rgb_b");
}
}
}
}

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Loading

0 comments on commit 42fa45b

Please sign in to comment.