Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

enable ego vehicle indicator request for autoware #817

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

enable ego vehicle indicator request for autoware #817

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions Assets/Scripts/Bridge/Data/VehicleControlData.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ public class VehicleControlData
public float? SteerAngle;
public bool ShiftGearUp;
public bool ShiftGearDown;
public int? Indicator;

// apollo
public float? SteerRate;
Expand Down
6 changes: 3 additions & 3 deletions Assets/Scripts/Bridge/Ros/Messages/Autoware.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
*
*/

#pragma warning disable 0649
#pragma warning disable 0649

using System.Collections.Generic;

Expand Down Expand Up @@ -56,7 +56,7 @@ public class VehicleCmd
public steer_cmd _steer_cmd;
public accel_cmd _accel_cmd;
public brake_cmd _brake_cmd;
public lamp_cmd _lamp_cmd;
public lamp_cmd lamp_cmd;
public uint gear;
public uint mode;
public TwistStamped twist_cmd;
Expand All @@ -82,7 +82,7 @@ public class DetectedObject
public Twist acceleration;

public PointCloud2 pointcloud;

// public PolygonStamped convex_hull;
// public LaneArray candidate_trajectories;

Expand Down
20 changes: 15 additions & 5 deletions Assets/Scripts/Bridge/Ros/RosConversions.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -440,7 +440,7 @@ public static Odometry ConvertFrom(GpsOdometryData data)
{
x = 0.0,
y = 0.0,
z = - data.AngularVelocity.y,
z = -data.AngularVelocity.y,
}
},
}
Expand Down Expand Up @@ -534,6 +534,15 @@ public static VehicleControlData ConvertTo(Autoware.VehicleCmd data)
shiftDown = true;
}


int indicator = 0;
if (data.lamp_cmd != null)
{
var lbrink = 0 < data.lamp_cmd.l ? 1 : 0;
var rbrink = 0 < data.lamp_cmd.r ? 1 : 0;
indicator = ((rbrink & 0x01) << 1) | (lbrink & 0x01);
}

return new VehicleControlData()
{
TimeStampSec = ConvertTime(data.header.stamp),
Expand All @@ -544,6 +553,7 @@ public static VehicleControlData ConvertTo(Autoware.VehicleCmd data)
SteerAngle = (float)data.ctrl_cmd.steering_angle,
ShiftGearUp = shiftUp,
ShiftGearDown = shiftDown,
Indicator = indicator
};
}

Expand All @@ -568,7 +578,7 @@ public static VehicleControlData ConvertTo(Lgsvl.VehicleControlDataRos data)
wheelAngle = -MaxSteeringAngle;

// ratio between -MaxSteeringAngle and MaxSteeringAngle
var k = (float)(wheelAngle + MaxSteeringAngle) / (MaxSteeringAngle*2);
var k = (float)(wheelAngle + MaxSteeringAngle) / (MaxSteeringAngle * 2);

// target_wheel_angular_rate, target_gear are not supported on simulator side.
return new VehicleControlData()
Expand Down Expand Up @@ -619,11 +629,11 @@ public static Imu ConvertFrom(ImuData data)
},

orientation = Convert(data.Orientation),
orientation_covariance = new double[9]{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001},
orientation_covariance = new double[9] { 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 },
angular_velocity = ConvertToVector(data.AngularVelocity),
angular_velocity_covariance = new double[9]{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001},
angular_velocity_covariance = new double[9] { 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 },
linear_acceleration = ConvertToVector(data.Acceleration),
linear_acceleration_covariance = new double[9]{0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001},
linear_acceleration_covariance = new double[9] { 0.0001, 0, 0, 0, 0.0001, 0, 0, 0, 0.0001 },
};
}

Expand Down
27 changes: 24 additions & 3 deletions Assets/Scripts/Controllers/VehicleController.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,13 +84,34 @@ private void UpdateInput()
if (sticky) return;

SteerInput = AccelInput = BrakeInput = 0f;

// get all inputs
foreach (var input in inputs)
{
SteerInput += input.SteerInput;
AccelInput += input.AccelInput;
BrakeInput += input.BrakeInput;

var vcs = input as Simulator.Sensors.VehicleControlSensor;
if (vcs != null)
{
var hazard = vcs.Indicator == 3;
var rightTurn = vcs.Indicator == 2;
var leftTurn = vcs.Indicator == 1;
if (actions.HazardLights != hazard)
{
actions.HazardLights = hazard;
}
else if (actions.LeftTurnSignal != leftTurn)
{
actions.LeftTurnSignal = leftTurn;
}
else if (actions.RightTurnSignal != rightTurn)
{
actions.RightTurnSignal = rightTurn;
}
}

}

// clamp if over
Expand All @@ -102,7 +123,7 @@ private void UpdateInput()
private void UpdateInputAPI()
{
if (!sticky) return;

SteerInput = stickySteering;
AccelInput = stickAcceleraton;
}
Expand All @@ -125,7 +146,7 @@ private void UpdateLights()
{
if (SteerInput > -turnSignalOffThreshold)
actions.LeftTurnSignal = resetTurnIndicator = false;

}
else
{
Expand Down
9 changes: 8 additions & 1 deletion Assets/Scripts/Sensors/VehicleControlSensor.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,8 @@ public class VehicleControlSensor : SensorBase, IVehicleInputs
public float AccelInput { get; private set; } = 0f;
public float BrakeInput { get; private set; } = 0f;

public int Indicator { get; private set; } = 0;

float ADAccelInput = 0f;
float ADSteerInput = 0f;

Expand Down Expand Up @@ -107,6 +109,11 @@ public override void OnBridgeSetup(IBridge bridge)
ADAccelInput = ActualLinVel < data.Velocity.GetValueOrDefault() ? linMag : -linMag;
ADSteerInput = -Mathf.Clamp(data.SteerAngularVelocity.GetValueOrDefault() * 0.5f, -1f, 1f);
}

if (data.Indicator.HasValue)
{
Indicator = data.Indicator.Value;
}
}
else if (data.SteerRate.HasValue) // apollo
{
Expand All @@ -130,7 +137,7 @@ public override void OnBridgeSetup(IBridge bridge)
var sgn = Mathf.Sign(steeringTarget - steeringAngle);
var steeringRate = data.SteerRate.GetValueOrDefault() * sgn;
steeringAngle += steeringRate * dt;

if (sgn != steeringTarget - steeringAngle) // to prevent oversteering
steeringAngle = steeringTarget;

Expand Down