Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.2.0 (#264)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Nov 6, 2023
2 parents b228ab4 + 423c6f8 commit c0c4cc8
Show file tree
Hide file tree
Showing 30 changed files with 297 additions and 190 deletions.
52 changes: 11 additions & 41 deletions Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,8 +10,8 @@

public class JointControlPlugin : CLOiSimPlugin
{
private List<TF> staticTfList = new List<TF>();
private List<TF> tfList = new List<TF>();
private string robotDescription_ = "<?xml version='1.0' ?><sdf></sdf>";
private SensorDevices.JointCommand jointCommand = null;
private SensorDevices.JointState jointState = null;

Expand Down Expand Up @@ -49,6 +49,9 @@ protected override void OnStart()
}

LoadJoints();

robotDescription_ = "<?xml version='1.0' ?><sdf>" + GetPluginParameters().ParentRawXml() + "</sdf>";
// UnityEngine.Debug.Log(robotDescription_);
}

protected override void OnReset()
Expand All @@ -64,11 +67,10 @@ private void LoadJoints()
{
foreach (var jointName in joints)
{
var parentFrameId = GetPluginParameters().GetAttributeInPath<string>("joints/joint[text()='" + jointName + "']", "parent_frame_id");

// UnityEngine.Debug.Log("Joints loaded "+ jointName);
if (jointState.AddTargetJoint(jointName, out var targetLink, out var isStatic))
{
var parentFrameId = GetPluginParameters().GetAttributeInPath<string>("joints/joint[text()='" + jointName + "']", "parent_frame_id");
var jointParentLinkName = (parentFrameId == null) ? targetLink.JointParentLinkName : parentFrameId;
var tf = new TF(targetLink, targetLink.JointChildLinkName, jointParentLinkName);
if (isStatic)
Expand All @@ -91,58 +93,26 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any
{
switch (requestType)
{
case "request_static_transforms":
SetStaticTransformsResponse(ref response);
break;

case "reset_odometry":
Reset();
SetEmptyResponse(ref response);
case "robot_description":
SetRobotDescription(ref response);
break;

default:
break;
}
}

private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info)
private void SetRobotDescription(ref DeviceMessage msRos2Info)
{
if (msRos2Info == null)
{
return;
}

var ros2CommonInfo = new messages.Param();
ros2CommonInfo.Name = "static_transforms";
ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };

foreach (var tf in staticTfList)
{
var ros2StaticTransformLink = new messages.Param();
ros2StaticTransformLink.Name = "parent_frame_id";
ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId };

{
var tfPose = tf.GetPose();

var poseMessage = new messages.Pose();
poseMessage.Position = new messages.Vector3d();
poseMessage.Orientation = new messages.Quaternion();

poseMessage.Name = tf.childFrameId;
DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position);
DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation);

var ros2StaticTransformElement = new messages.Param();
ros2StaticTransformElement.Name = "pose";
ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage };

ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement);
// Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation);
}

ros2CommonInfo.Childrens.Add(ros2StaticTransformLink);
}
ros2CommonInfo.Name = "description";
ros2CommonInfo.Value = new Any { Type = Any.ValueType.String };
ros2CommonInfo.Value.StringValue = robotDescription_;

msRos2Info.SetMessage<messages.Param>(ros2CommonInfo);
}
Expand Down
4 changes: 3 additions & 1 deletion Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,9 @@ MSG+="sensor_noise "
MSG+="world_stats log_playback_stats "
MSG+="request response "
MSG+="perception perception_v "
MSG+="pid joint_cmd wrench joint_wrench joint_state joint_state_v "
MSG+="pid wrench joint_wrench "
MSG+="joint_state joint_state_v "
MSG+="joint_cmd joint_cmd_v "
MSG+="transform_stamped "
# MSG+=" "

Expand Down
4 changes: 2 additions & 2 deletions Assets/Scripts/CLOiSimPlugins/Messages/color.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,10 @@ public partial class Color : global::ProtoBuf.IExtensible
public float B { get; set; }

[global::ProtoBuf.ProtoMember(5, Name = @"a")]
[global::System.ComponentModel.DefaultValue(1)]
[global::System.ComponentModel.DefaultValue(1f)]
public float A
{
get => __pbn__A ?? 1;
get => __pbn__A ?? 1f;
set => __pbn__A = value;
}
public bool ShouldSerializeA() => __pbn__A != null;
Expand Down
30 changes: 30 additions & 0 deletions Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,30 @@
// <auto-generated>
// This file was generated by a tool; you should avoid making direct changes.
// Consider using 'partial classes' to extend these types
// Input: joint_cmd_v.proto
// </auto-generated>

#region Designer generated code
#pragma warning disable CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192
namespace cloisim.msgs
{

[global::ProtoBuf.ProtoContract(Name = @"JointCmd_V")]
public partial class JointCmdV : global::ProtoBuf.IExtensible
{
private global::ProtoBuf.IExtension __pbn__extensionData;
global::ProtoBuf.IExtension global::ProtoBuf.IExtensible.GetExtensionObject(bool createIfMissing)
=> global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing);

[global::ProtoBuf.ProtoMember(1, Name = @"time", IsRequired = true)]
public Time Time { get; set; }

[global::ProtoBuf.ProtoMember(2, Name = @"JointCmd")]
public global::System.Collections.Generic.List<JointCmd> JointCmds { get; } = new global::System.Collections.Generic.List<JointCmd>();

}

}

#pragma warning restore CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192
#endregion
11 changes: 11 additions & 0 deletions Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs.meta
View file

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

28 changes: 14 additions & 14 deletions Assets/Scripts/CLOiSimPlugins/Messages/pid.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,76 +17,76 @@ public partial class Pid : global::ProtoBuf.IExtensible
=> global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing);

[global::ProtoBuf.ProtoMember(1, Name = @"target")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double Target
{
get => __pbn__Target ?? 0;
get => __pbn__Target ?? 0d;
set => __pbn__Target = value;
}
public bool ShouldSerializeTarget() => __pbn__Target != null;
public void ResetTarget() => __pbn__Target = null;
private double? __pbn__Target;

[global::ProtoBuf.ProtoMember(2, Name = @"p_gain")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double PGain
{
get => __pbn__PGain ?? 0;
get => __pbn__PGain ?? 0d;
set => __pbn__PGain = value;
}
public bool ShouldSerializePGain() => __pbn__PGain != null;
public void ResetPGain() => __pbn__PGain = null;
private double? __pbn__PGain;

[global::ProtoBuf.ProtoMember(3, Name = @"i_gain")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double IGain
{
get => __pbn__IGain ?? 0;
get => __pbn__IGain ?? 0d;
set => __pbn__IGain = value;
}
public bool ShouldSerializeIGain() => __pbn__IGain != null;
public void ResetIGain() => __pbn__IGain = null;
private double? __pbn__IGain;

[global::ProtoBuf.ProtoMember(4, Name = @"d_gain")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double DGain
{
get => __pbn__DGain ?? 0;
get => __pbn__DGain ?? 0d;
set => __pbn__DGain = value;
}
public bool ShouldSerializeDGain() => __pbn__DGain != null;
public void ResetDGain() => __pbn__DGain = null;
private double? __pbn__DGain;

[global::ProtoBuf.ProtoMember(5, Name = @"i_max")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double IMax
{
get => __pbn__IMax ?? 0;
get => __pbn__IMax ?? 0d;
set => __pbn__IMax = value;
}
public bool ShouldSerializeIMax() => __pbn__IMax != null;
public void ResetIMax() => __pbn__IMax = null;
private double? __pbn__IMax;

[global::ProtoBuf.ProtoMember(6, Name = @"i_min")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double IMin
{
get => __pbn__IMin ?? 0;
get => __pbn__IMin ?? 0d;
set => __pbn__IMin = value;
}
public bool ShouldSerializeIMin() => __pbn__IMin != null;
public void ResetIMin() => __pbn__IMin = null;
private double? __pbn__IMin;

[global::ProtoBuf.ProtoMember(7, Name = @"limit")]
[global::System.ComponentModel.DefaultValue(0)]
[global::System.ComponentModel.DefaultValue(0d)]
public double Limit
{
get => __pbn__Limit ?? 0;
get => __pbn__Limit ?? 0d;
set => __pbn__Limit = value;
}
public bool ShouldSerializeLimit() => __pbn__Limit != null;
Expand Down
49 changes: 0 additions & 49 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,10 @@
using System.Collections.Generic;
using System.Text;
using Any = cloisim.msgs.Any;
using messages = cloisim.msgs;
using UnityEngine;

public class MicomPlugin : CLOiSimPlugin
{
private List<TF> staticTfList = new List<TF>();
private List<TF> tfList = new List<TF>();
private SensorDevices.MicomCommand micomCommand = null;
private SensorDevices.MicomSensor micomSensor = null;
Expand Down Expand Up @@ -184,10 +182,6 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any
{
switch (requestType)
{
case "request_static_transforms":
SetStaticTransformsResponse(ref response);
break;

case "reset_odometry":
Reset();
SetEmptyResponse(ref response);
Expand All @@ -197,47 +191,4 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any
break;
}
}

private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info)
{
if (msRos2Info == null)
{
return;
}

var ros2CommonInfo = new messages.Param();
ros2CommonInfo.Name = "static_transforms";
ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };

foreach (var tf in staticTfList)
{
var ros2StaticTransformLink = new messages.Param();
ros2StaticTransformLink.Name = "parent_frame_id";
ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId };

{
var tfPose = tf.GetPose();
// Debug.Log(tf.parentFrameId + " <= " + tf.childFrameId + " = " + tf.link.JointAxis + ", " + tfPose);

var poseMessage = new messages.Pose();
poseMessage.Position = new messages.Vector3d();
poseMessage.Orientation = new messages.Quaternion();

poseMessage.Name = tf.childFrameId;
DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position);
DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation);

var ros2StaticTransformElement = new messages.Param();
ros2StaticTransformElement.Name = "pose";
ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage };

ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement);
// Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation);
}

ros2CommonInfo.Childrens.Add(ros2StaticTransformLink);
}

msRos2Info.SetMessage<messages.Param>(ros2CommonInfo);
}
}
47 changes: 47 additions & 0 deletions Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,10 @@ private void SetCustomHandleRequestMessage()
}
break;

case "request_static_transforms":
SetStaticTransformsResponse(ref response);
break;

default:
HandleCustomRequestMessage(requestType, requestValue, ref response);
break;
Expand All @@ -178,6 +182,49 @@ private void SetCustomHandleRequestMessage()
};
}

private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info)
{
if (msRos2Info == null)
{
return;
}

var ros2CommonInfo = new messages.Param();
ros2CommonInfo.Name = "static_transforms";
ros2CommonInfo.Value = new Any { Type = Any.ValueType.None };

foreach (var tf in staticTfList)
{
var ros2StaticTransformLink = new messages.Param();
ros2StaticTransformLink.Name = "parent_frame_id";
ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId };

{
var tfPose = tf.GetPose();
// Debug.Log(tf.parentFrameId + " <= " + tf.childFrameId + " = " + tf.link.JointAxis + ", " + tfPose);

var poseMessage = new messages.Pose();
poseMessage.Position = new messages.Vector3d();
poseMessage.Orientation = new messages.Quaternion();

poseMessage.Name = tf.childFrameId;
DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position);
DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation);

var ros2StaticTransformElement = new messages.Param();
ros2StaticTransformElement.Name = "pose";
ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage };

ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement);
// Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation);
}

ros2CommonInfo.Childrens.Add(ros2StaticTransformLink);
}

msRos2Info.SetMessage<messages.Param>(ros2CommonInfo);
}

protected virtual void HandleCustomRequestMessage(in string requestType, in List<messages.Param> requestChildren, ref DeviceMessage response) { }
protected virtual void HandleCustomRequestMessage(in string requestType, in Any requestValue, ref DeviceMessage response) { }
}
2 changes: 2 additions & 0 deletions Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,8 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin
public string modelName { get; protected set; } = string.Empty;
public string partsName { get; protected set; } = string.Empty;

protected List<TF> staticTfList = new List<TF>();

private Pose pluginPose = Pose.identity;

private SDF.Plugin pluginParameters;
Expand Down
Loading

0 comments on commit c0c4cc8

Please sign in to comment.