Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.8.0 (#320)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Sep 12, 2024
2 parents a42a60a + fc94590 commit 53cee32
Show file tree
Hide file tree
Showing 46 changed files with 1,030 additions and 585 deletions.
4 changes: 2 additions & 2 deletions Assets/Resources/Shader/GeometryGrass.shader
View file
Original file line number Diff line number Diff line change
Expand Up @@ -358,8 +358,8 @@ Shader "Custom/GeometryGrass"
#endif

float width = lerp(_BladeWidthMin, _BladeWidthMax, rand(pos.xzy) * falloff);
float height = lerp(_BladeHeightMin, _BladeHeightMax, rand(pos.zyx) * falloff);
float forward = rand(pos.yyz) * _BladeBendDistance;
float height = lerp(_BladeHeightMin, _BladeHeightMax * grassVisibility, rand(pos.zyx) * falloff);
float forward = rand(pos.yyz) * _BladeBendDistance * grassVisibility;

#ifdef DRY_GRASS_ON
float dryRate = tex2Dlod(_DryGrassMap, float4(-input[0].uv, 0, 0)).r;
Expand Down
114 changes: 77 additions & 37 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,8 @@

using System.Collections.Generic;
using System.Text;
using System.Linq;
using System;
using Any = cloisim.msgs.Any;
using UnityEngine;

Expand Down Expand Up @@ -82,45 +84,14 @@ private void SetupMicom()
}
_micomSensor.SetUpdateRate(updateRate);

var wheelRadius = GetPluginParameters().GetValue<float>("wheel/radius");

if (GetPluginParameters().IsValidNode("wheel/tread"))
{
Debug.LogWarning("<wheel/tread> will be depreacted!! please use wheel/separation");
}

var wheelTread = GetPluginParameters().GetValue<float>("wheel/tread"); // TODO: to be deprecated
var wheelSeparation = GetPluginParameters().GetValue<float>("wheel/separation", wheelTread);

_motorControl.SetWheelInfo(wheelRadius, wheelSeparation);

var wheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='left']", string.Empty);
var wheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='right']", string.Empty);
var rearWheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_left']", string.Empty);
var rearWheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_right']", string.Empty);

if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty))
if (GetPluginParameters().IsValidNode("wheel"))
{
SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName);
}
else
{
SetWheel(wheelLeftName, wheelRightName);
SetWheel();
}

if (GetPluginParameters().IsValidNode("wheel/PID"))
if (GetPluginParameters().IsValidNode("mowing"))
{
var P = GetPluginParameters().GetValue<float>("wheel/PID/kp");
var I = GetPluginParameters().GetValue<float>("wheel/PID/ki");
var D = GetPluginParameters().GetValue<float>("wheel/PID/kd");

var iMin = GetPluginParameters().GetValue<float>("wheel/PID/limit/integral/min", -100);
var iMax = GetPluginParameters().GetValue<float>("wheel/PID/limit/integral/max", 100);
var outputMin = GetPluginParameters().GetValue<float>("wheel/PID/limit/output/min", -1000);
var outputMax = GetPluginParameters().GetValue<float>("wheel/PID/limit/output/max", 1000);
// Debug.Log(iMin + ", " + iMax + ", " + outputMin + ", " + outputMax);

_motorControl.SetPID(P, I, D, iMin, iMax, outputMin, outputMax);
SetMowing();
}

if (GetPluginParameters().IsValidNode("battery"))
Expand Down Expand Up @@ -156,6 +127,75 @@ private void SetupMicom()
}
}

private void SetWheel()
{
var wheelRadius = GetPluginParameters().GetValue<float>("wheel/radius");

if (GetPluginParameters().IsValidNode("wheel/tread"))
{
Debug.LogWarning("<wheel/tread> will be depreacted!! please use wheel/separation");
}

var wheelTread = GetPluginParameters().GetValue<float>("wheel/tread"); // TODO: to be deprecated
var wheelSeparation = GetPluginParameters().GetValue<float>("wheel/separation", wheelTread);

_motorControl.SetWheelInfo(wheelRadius, wheelSeparation);

var wheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='left']", string.Empty);
var wheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='right']", string.Empty);
var rearWheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_left']", string.Empty);
var rearWheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_right']", string.Empty);

if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty))
{
SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName);
}
else
{
SetWheel(wheelLeftName, wheelRightName);
}

if (GetPluginParameters().IsValidNode("wheel/PID"))
{
SetWheelPID();
}
}

private void SetWheelPID()
{
var P = GetPluginParameters().GetValue<float>("wheel/PID/kp");
var I = GetPluginParameters().GetValue<float>("wheel/PID/ki");
var D = GetPluginParameters().GetValue<float>("wheel/PID/kd");

var iMin = GetPluginParameters().GetValue<float>("wheel/PID/limit/integral/min", -100);
var iMax = GetPluginParameters().GetValue<float>("wheel/PID/limit/integral/max", 100);
var outputMin = GetPluginParameters().GetValue<float>("wheel/PID/limit/output/min", -1000);
var outputMax = GetPluginParameters().GetValue<float>("wheel/PID/limit/output/max", 1000);
// Debug.Log(iMin + ", " + iMax + ", " + outputMin + ", " + outputMax);

_motorControl.SetPID(P, I, D, iMin, iMax, outputMin, outputMax);
}

private void SetMowing()
{
var targetBladeName = GetPluginParameters().GetAttributeInPath<string>("mowing/blade", "target");
if (!string.IsNullOrEmpty(targetBladeName))
{
var linkHelpers = GetComponentsInChildren<SDF.Helper.Link>();
var targetBlade = linkHelpers.FirstOrDefault(x => x.name == targetBladeName);

if (targetBlade != null)
{
var mowingBlade = targetBlade.gameObject.AddComponent<MowingBlade>();

mowingBlade.HeightMin = GetPluginParameters().GetValue<float>("mowing/blade/height/min", 0f);
mowingBlade.HeightMax = GetPluginParameters().GetValue<float>("mowing/blade/height/max", 0.1f);
mowingBlade.RevSpeedMax = GetPluginParameters().GetValue<UInt16>("mowing/blade/rev_speed/max", 1000);
mowingBlade.Height = 0;
}
}
}

private void SetBattery()
{
if (GetPluginParameters().GetValues<string>("battery/voltage", out var batteryList))
Expand Down Expand Up @@ -256,7 +296,7 @@ private void LoadStaticTF()
{
var parentFrameId = GetPluginParameters().GetAttributeInPath<string>("ros2/static_transforms/link[text()='" + link + "']", "parent_frame_id", "base_link");

(var modelName, var linkName) = SDF2Unity.GetModelLinkName(link);
var (modelName, linkName) = SDF2Unity.GetModelLinkName(link);

foreach (var linkHelper in _linkHelperInChildren)
{
Expand Down Expand Up @@ -287,7 +327,7 @@ private void LoadTF()
{
var parentFrameId = GetPluginParameters().GetAttributeInPath<string>("ros2/transforms/link[text()='" + link + "']", "parent_frame_id", "base_link");

(var modelName, var linkName) = SDF2Unity.GetModelLinkName(link);
var (modelName, linkName) = SDF2Unity.GetModelLinkName(link);

foreach (var linkHelper in _linkHelperInChildren)
{
Expand Down
Loading

0 comments on commit 53cee32

Please sign in to comment.