public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
ApolloWriterImu = Bridge.AddWriter <Ros.Apollo.Imu>(ImuTopic);
ApolloWriterCorrectedImu = Bridge.AddWriter <Ros.CorrectedImu>(ApolloIMUOdometryTopic);
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
Apollo35WriterImu = Bridge.AddWriter <Apollo.Drivers.Gnss.Imu>(ImuTopic);
Apollo35WriterCorrectedImu = Bridge.AddWriter <Apollo.Localization.CorrectedImu>(ApolloIMUOdometryTopic);
}
else if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
AutowareWriterImu = Bridge.AddWriter <Ros.Imu>(ImuTopic);
AutowareWriterOdometry = Bridge.AddWriter <Ros.Odometry>(OdometryTopic);
Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(ResetOdometry, msg =>
{
odomPosition = new Vector3(0f, 0f, 0f);
return(new Ros.Srv.Empty());
});
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddService <Ros.Srv.SetEffect, Ros.Srv.SetEffectResponse>(buzzerTopicName, msg =>
{
if (msg.data == 0)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerOff);
}
else if (msg.data == 1)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerOne);
}
else if (msg.data == 2)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerTwo);
}
else if (msg.data == 3)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerThree);
}
return(new Ros.Srv.SetEffectResponse()
{
success = true, message = "message"
});
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddReader <Ros.Twist>(CMD_VEL_TOPIC, (Ros.Twist msg) =>
{
float WHEEL_SEPARATION = 0.515f;
//float WHEEL_DIAMETER = 0.39273163f;
float SCALING_RATIO = 0.208f;
// Assuming that we only get linear in x and angular in z
double v = msg.linear.x;
double w = msg.angular.z;
wheelLeftVel = SCALING_RATIO * (float)(v - w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = SCALING_RATIO * (float)(v + w * 0.5 * WHEEL_SEPARATION);
});
Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(CENTER_GRIPPER_SRV, msg =>
{
hook.CenterHook();
return(new Ros.Srv.Empty());
});
Bridge.AddService <Ros.Srv.SetBool, Ros.Srv.SetBoolResponse>(ATTACH_GRIPPER_SRV, msg =>
{
hook.EngageHook(msg.data);
return(new Ros.Srv.SetBoolResponse()
{
success = true, message = ""
});
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
ImageIsBeingSent = false;
#if USE_COMPRESSED
if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
CyberVideoWriter = Bridge.AddWriter <Apollo.Drivers.CompressedImage>(TopicName);
}
else
{
VideoWriter = Bridge.AddWriter <Ros.CompressedImage>(TopicName);
}
#else
if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
// TODO
}
else
{
VideoWriter = Bridge.AddWriter <Ros.Image>(TopicName);
}
#endif
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
AutowareWriterSentence = Bridge.AddWriter <Ros.Sentence>(AutowareTopic);
AutowareWriterOdometry = Bridge.AddWriter <Ros.Odometry>(AutowareOdometryTopic);
}
else if (targetEnv == ROSTargetEnvironment.APOLLO)
{
ApolloWriterGnssBestPose = Bridge.AddWriter <Ros.GnssBestPose>(ApolloTopic);
ApolloWriterGps = Bridge.AddWriter <Ros.Gps>(ApolloGPSOdometryTopic);
}
else if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
Apollo35WriterGnssBestPose = Bridge.AddWriter <Apollo.Drivers.Gnss.GnssBestPose>(ApolloTopic);
Apollo35WriterGps = Bridge.AddWriter <Apollo.Localization.Gps>(ApolloGPSOdometryTopic);
Apollo35WriterInsStat = Bridge.AddWriter <Apollo.Drivers.Gnss.InsStat>(ApolloInsStatTopic);
}
seq = 0;
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
ApolloWriterPointCloud2 = Bridge.AddWriter <Ros.PointCloud2>(ApolloTopicName);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
AutowareWriterPointCloud2 = Bridge.AddWriter <Ros.PointCloud2>(AutowareTopicName);
}
else if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
Apollo35WriterPointCloud2 = Bridge.AddWriter <Apollo.Drivers.PointCloud>(ApolloTopicName);
}
else
{
if (RayCount == 1)
{
WriterLaserScan = Bridge.AddWriter <Ros.LaserScan>(TopicName);
}
else
{
WriterPointCloud2 = Bridge.AddWriter <Ros.PointCloud2>(TopicName);
}
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddService <Ros.Srv.Int, Ros.Srv.Int>(buzzerTopicName, msg =>
{
if (msg.data == 0)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerOff);
}
else if (msg.data == 1)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerOne);
}
else if (msg.data == 2)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerTwo);
}
else if (msg.data == 3)
{
SetBuzzerMode(BuzzerModeTypes.BuzzerThree);
}
return(new Ros.Srv.Int()
{
data = 1
});
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddReader <Ros.Vector3>(ResetTopic, msg =>
{
var position = GetPosition(GpsDevice.targetEnv, msg.x, msg.y);
int mask = 1 << LayerMask.NameToLayer("Ground And Road");
RaycastHit hit;
if (Physics.Raycast(position + new Vector3(0, 100, 0), new Vector3(0, -1, 0), out hit, Mathf.Infinity, mask))
{
position = hit.point;
position.y += 0.01f;
}
else
{
position.y += 20.0f;
}
var angle = (float)msg.z * Mathf.Rad2Deg - GpsDevice.Angle;
var rotation = Quaternion.AngleAxis(angle, Vector3.up);
// reset position, rotation, velocity and angular velocity
GpsDevice.Agent.GetComponent <VehicleInputController>().controller.ResetSavedPosition(position, rotation);
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
ApolloWriterContiRadar = Bridge.AddWriter <Ros.Apollo.Drivers.ContiRadar>(ApolloTopicName);
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
WriterPointCloud2 = Bridge.AddWriter <Ros.PointCloud2>(TopicName);
};
}
void Awake()
{
Bridge = new Comm.Ros.RosBridge();
ls = new LidarSensor();
if (Bridge.Status == Comm.BridgeStatus.Disconnected)
{
Bridge.Connect(Address, Port, 1);
ls.OnBridgeAvailable(Bridge);
}
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Debug.Log("rosbridge for /clock available");
Bridge = bridge;
//Bridge.AddPublisher(this);
Bridge.OnConnected += () =>
{
RosClockWriter = Bridge.AddWriter <Ros.Clock>(SimulatorTopic);
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
UnityTimeWriter = Bridge.AddWriter <float>(UNITY_TIME_TOPIC);
CarInfoWriter = Bridge.AddWriter <Ros.Pose>(CAR_INFO_TOPIC);
SimulatorCurrentVelocityWriter = Bridge.AddWriter <Ros.TwistStamped>(SIM_CUR_VELOCITY_TOPIC);
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
AutowareWriterLaserScan = Bridge.AddWriter <Ros.LaserScan>(topicName);
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddReader <Ros.control_command>(VehicleInputController.APOLLO_CMD_TOPIC, msg =>
{
Wheel.autonomousBehavior = SteerWheelAutonomousFeedbackBehavior.OutputOnly;
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
ImageIsBeingSent = false;
#if USE_COMPRESSED
VideoWriter = Bridge.AddWriter <Ros.CompressedImage>(TopicName);
#else
VideoWriter = Bridge.AddWriter <Ros.Image>(TopicName);
#endif
};
}
public static void AssignBridge(GameObject agent, Comm.Bridge bridge)
{
var components = agent.GetComponentsInChildren(typeof(Component));
foreach (var component in components)
{
var ros = component as Comm.BridgeClient;
if (ros != null)
{
ros.OnBridgeAvailable(bridge);
}
}
}
public RosBridgeConnector(AgentSetup type)
{
agentType = type;
lastEnvironment = agentType.TargetRosEnv;
if (type.TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
Bridge = new Comm.Cyber.CyberBridge();
}
else
{
Bridge = new Comm.Ros.RosBridge();
}
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Debug.Log("Init V2XBridge");//HW_start
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
SV2x = Bridge.AddWriter <Ros.V2x_Spat>(Spat_Topic);
BV2x = Bridge.AddWriter <Ros.V2x_BSM>(Bsm_Topic);
MV2x = Bridge.AddWriter <Ros.V2x_MAP>(Map_Topic);
TV2x = Bridge.AddWriter <Ros.V2x_TIM>(Tim_Topic);
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
Apollo35WriterContiRadar = Bridge.AddWriter <Apollo.Drivers.ContiRadar>(ApolloTopicName);
}
else
{
ApolloWriterContiRadar = Bridge.AddWriter <Ros.Apollo.Drivers.ContiRadar>(ApolloTopicName);
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddService <Ros.Srv.SetStateOfCharge, Ros.Srv.SetStateOfChargeResponse>(batteryTopicName, msg =>
{
SetBattery(msg.data);
SetBatterySlider();
return(new Ros.Srv.SetStateOfChargeResponse()
{
success = true, message = "message"
});
});
};
}
public void Disconnect()
{
connectTime = Time.time + 1.0f;
Bridge.Disconnect();
if (lastEnvironment != agentType.TargetRosEnv)
{
if (agentType.TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
Bridge = new Comm.Cyber.CyberBridge();
}
else
{
Bridge = new Comm.Ros.RosBridge();
}
lastEnvironment = agentType.TargetRosEnv;
}
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Debug.Log("CAN bus not implemented in Autoware (yet). Nothing to publish.");
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
ApolloChassisWriter = Bridge.AddWriter <Ros.Apollo.ChassisMsg>(ApolloTopic);
}
seq = 0;
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
AutowareWriterSentence = Bridge.AddWriter <Ros.Sentence>(AutowareTopic);
AutowareWriterOdometry = Bridge.AddWriter <Ros.Odometry>(AutowareOdometryTopic);
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
ApolloWriterGnssBestPose = Bridge.AddWriter <Ros.GnssBestPose>(ApolloTopic);
ApolloWriterGps = Bridge.AddWriter <Ros.Gps>(ApolloGPSOdometryTopic);
}
seq = 0;
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddService <Ros.Srv.Int, Ros.Srv.Int>(topLightTopicName, msg =>
{
switch (msg.data)
{
case 0:
SetTopLightMode(false);
break;
case 1:
SetTopLightMode(true);
break;
}
return(new Ros.Srv.Int()
{
data = 1
});
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
Bridge.AddService <Ros.Srv.SetEffect, Ros.Srv.SetEffectResponse>(topLightTopicName, msg =>
{
switch (msg.data)
{
case 0:
SetTopLightMode(false);
break;
case 1:
SetTopLightMode(true);
break;
}
return(new Ros.Srv.SetEffectResponse()
{
success = true, message = "message"
});
});
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO || targetEnv == ROSTargetEnvironment.LGSVL)
{
DectectedObjectArrayWriter = Bridge.AddWriter <Ros.Detection2DArray>(objects2DTopicName);
}
if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
Apollo35DetectedObjectArrayWriter = Bridge.AddWriter <Apollo.Common.Detection2DArray>(objects2DTopicName);
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.AddReader <Ros.DetectedObjectArray>(autowareCameraDetectionTopicName, msg =>
{
if (!isCameraPredictionEnabled || cameraPredictedObjects == null)
{
return;
}
foreach (Ros.DetectedObject obj in msg.objects)
{
var label = obj.label;
if (label == "person")
{
label = "pedestrian"; // Autoware label as person
}
Ros.Detection2D obj_converted = new Ros.Detection2D()
{
header = new Ros.Header()
{
stamp = new Ros.Time()
{
secs = obj.header.stamp.secs,
nsecs = obj.header.stamp.nsecs,
},
seq = obj.header.seq,
frame_id = obj.header.frame_id,
},
id = obj.id,
label = label,
score = obj.score,
bbox = new Ros.BoundingBox2D()
{
x = obj.x + obj.width / 2, // Autoware (x, y) point at top-left corner
y = obj.y + obj.height / 2,
width = obj.width,
height = obj.height,
},
velocity = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = obj.velocity.linear.x,
y = 0,
z = 0,
},
angular = new Ros.Vector3()
{
x = 0,
y = 0,
z = obj.velocity.angular.z,
},
},
};
cameraPredictedObjects.Add(obj_converted);
}
cameraPredictedVisuals = cameraPredictedObjects.ToList();
cameraPredictedObjects.Clear();
});
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO || targetEnv == ROSTargetEnvironment.LGSVL)
{
DetectedObjectArrayWriter = Bridge.AddWriter <Ros.Detection3DArray>(objects3DTopicName);
}
if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
Apollo35DetectedObjectArrayWriter = Bridge.AddWriter <Apollo.Common.Detection3DArray>(objects3DTopicName);
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.AddReader <Ros.DetectedObjectArray>(autowareLidarDetectionTopicName, msg =>
{
if (!isLidarPredictionEnabled || lidarPredictedObjects == null)
{
return;
}
foreach (Ros.DetectedObject obj in msg.objects)
{
Ros.Detection3D obj_converted = new Ros.Detection3D()
{
header = new Ros.Header()
{
stamp = new Ros.Time()
{
secs = obj.header.stamp.secs,
nsecs = obj.header.stamp.nsecs,
},
seq = obj.header.seq,
frame_id = obj.header.frame_id,
},
id = obj.id,
label = obj.label,
score = obj.score,
bbox = new Ros.BoundingBox3D()
{
position = new Ros.Pose()
{
position = new Ros.Point()
{
x = obj.pose.position.x,
y = obj.pose.position.y,
z = obj.pose.position.z,
},
orientation = new Ros.Quaternion()
{
x = obj.pose.orientation.x,
y = obj.pose.orientation.y,
z = obj.pose.orientation.z,
w = obj.pose.orientation.w,
},
},
size = new Ros.Vector3()
{
x = obj.dimensions.x,
y = obj.dimensions.y,
z = obj.dimensions.z,
},
},
velocity = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = obj.velocity.linear.x,
y = 0,
z = 0,
},
angular = new Ros.Vector3()
{
x = 0,
y = 0,
z = obj.velocity.angular.z,
},
},
};
lidarPredictedObjects.Add(obj_converted);
}
lidarPredictedVisuals = lidarPredictedObjects.ToList();
lidarPredictedObjects.Clear();
});
}
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
// tugbot
Bridge.AddReader <Ros.Twist>(CMD_VEL_TOPIC,
(Ros.Twist msg) =>
{
float WHEEL_SEPARATION = 0.515f;
float WHEEL_DIAMETER = 0.39273163f;
float SCALING_RATIO = 0.208f;
// Assuming that we only get linear in x and angular in z
double v = msg.linear.x;
double w = msg.angular.z;
wheelLeftVel = SCALING_RATIO * (float)(v - w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = SCALING_RATIO * (float)(v + w * 0.5 * WHEEL_SEPARATION);
});
Bridge.AddReader <WheelsCmdStampedMsg>(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.AddReader <Ros.VehicleCmd>(AUTOWARE_CMD_TOPIC,
(Ros.VehicleCmd msg) =>
{
float WHEEL_SEPARATION = 0.1044197f;
//float WHEEL_DIAMETER = 0.065f;
float L_GAIN = 0.25f;
float A_GAIN = 8.0f;
// Assuming that we only get linear in x and angular in z
double v = msg.twist_cmd.twist.linear.x;
double w = msg.twist_cmd.twist.angular.z;
wheelLeftVel = (float)(L_GAIN * v - A_GAIN * w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = (float)(L_GAIN * v + A_GAIN * w * 0.5 * WHEEL_SEPARATION);
});
Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(CENTER_GRIPPER_SRV, msg =>
{
hook.CenterHook();
return(new Ros.Srv.Empty());
});
Bridge.AddService <Ros.Srv.SetBool, Ros.Srv.SetBoolResponse>(ATTACH_GRIPPER_SRV, msg =>
{
hook.EngageHook(msg.data);
return(new Ros.Srv.SetBoolResponse()
{
success = true, message = ""
});
});
string override_topic;
if (Bridge.Version == 1)
{
JostickRos1Writer = Bridge.AddWriter <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
JoystickOverrideTopicWriter = Bridge.AddWriter <BoolStamped>(override_topic);
Bridge.AddReader <BoolStamped>(override_topic, stamped =>
{
ManualControl = stamped.data;
});
if (FirstConnection)
{
var stamp = new BoolStamped()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "joystick",
},
data = true,
};
var topic = (Bridge.Version == 1) ? JOYSTICK_OVERRIDE_TOPIC_ROS1 : JOYSTICK_OVERRIDE_TOPIC_ROS2;
JoystickOverrideTopicWriter.Publish(stamp);
FirstConnection = false;
}
ManualControl = true;
};
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.AddReader(AUTOWARE_CMD_TOPIC, (System.Action <Ros.VehicleCmd>)((Ros.VehicleCmd msg) =>
{
lastAutoUpdate = Time.time;
bool pub_ctrl_cmd = false;
bool pub_gear_cmd = false;
var gearCmd = msg.gear;
if (gearCmd != 0)
{
pub_gear_cmd = true;
}
var ctrlCmd_linVel = msg.ctrl_cmd.linear_velocity;
var ctrlCmd_linAcc = msg.ctrl_cmd.linear_acceleration;
var ctrlCmd_steerAng = msg.ctrl_cmd.steering_angle;
if (ctrlCmd_linAcc == 0 && ctrlCmd_linVel == 0 && ctrlCmd_steerAng == 0)
{
pub_ctrl_cmd = false;
}
else
{
pub_ctrl_cmd = true;
}
if (!pub_ctrl_cmd && !pub_gear_cmd)
{
// using twist_cmd to control ego vehicle
var targetLinear = (float)msg.twist_cmd.twist.linear.x;
var targetAngular = (float)msg.twist_cmd.twist.angular.z;
if (!underKeyboardControl)
{
var linMag = Mathf.Abs(targetLinear - actualLinVel);
if (actualLinVel < targetLinear && !controller.InReverse)
{
autoInputAccel = Mathf.Clamp(linMag, 0, constAccel);
}
else if (actualLinVel > targetLinear && !controller.InReverse)
{
autoInputAccel = -Mathf.Clamp(linMag, 0, constAccel);
}
autoSteerAngle = -Mathf.Clamp(targetAngular * 0.5f, -constSteer, constSteer);
}
}
else
{
// using gear and ctrl_cmd to control ego vehicle
if (gearCmd == 64)
{
controller.GearboxShiftDown();
}
else // Switch to "Drive" for anything but "Reverse"
{
controller.GearboxShiftUp();
}
if (!underKeyboardControl)
{
// ignoring the control linear velocity for now.
autoSteerAngle = (float)ctrlCmd_steerAng; // angle should be in degrees
autoInputAccel = Mathf.Clamp((float)ctrlCmd_linAcc, -1, 1);
}
}
}));
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.AddReader <Ros.control_command>(APOLLO_CMD_TOPIC, (System.Action <Ros.control_command>)(msg =>
{
lastAutoUpdate = Time.time;
var pedals = GetComponent <PedalInputController>();
throttle = pedals.throttleInputCurve.Evaluate((float)msg.throttle / 100);
brake = pedals.brakeInputCurve.Evaluate((float)msg.brake / 100);
var linearAccel = throttle - brake;
var timeStamp = (float)msg.header.timestamp_sec;
var steeringTarget = -((float)msg.steering_target) / 100;
var dt = timeStamp - lastTimeStamp;
lastTimeStamp = timeStamp;
var steeringAngle = controller.steerInput;
var sgn = Mathf.Sign(steeringTarget - steeringAngle);
var steeringRate = (float)msg.steering_rate * sgn;
steeringAngle += steeringRate * dt;
// to prevent oversteering
if (sgn != steeringTarget - steeringAngle)
{
steeringAngle = steeringTarget;
}
if (!underKeyboardControl)
{
autoSteerAngle = steeringAngle;
autoInputAccel = linearAccel;
}
}));
}
else if (TargetRosEnv == ROSTargetEnvironment.LGSVL)
{
Bridge.AddReader <Ros.TwistStamped>(LANEFOLLOWING_CMD_TOPIC, (System.Action <Ros.TwistStamped>)(msg =>
{
lastAutoUpdate = Time.time;
autoSteerAngle = (float)msg.twist.angular.x;
}));
seq = 0;
LgsvlSimulatorCmdWriter = Bridge.AddWriter <Ros.TwistStamped>(SIMULATOR_CMD_TOPIC);
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
Bridge.AddReader <apollo.control.ControlCommand>(APOLLO_CMD_TOPIC, (System.Action <apollo.control.ControlCommand>)(msg =>
{
if (double.IsInfinity(msg.brake) || double.IsNaN(msg.brake) ||
double.IsInfinity(msg.throttle) || double.IsNaN(msg.throttle))
{
return;
}
lastAutoUpdate = Time.time;
var pedals = GetComponent <PedalInputController>();
throttle = pedals.throttleInputCurve.Evaluate((float)msg.throttle / 100);
brake = pedals.brakeInputCurve.Evaluate((float)msg.brake / 100);
var linearAccel = throttle - brake;
var timeStamp = (float)msg.header.timestamp_sec;
var steeringTarget = -((float)msg.steering_target) / 100;
var dt = timeStamp - lastTimeStamp;
lastTimeStamp = timeStamp;
var steeringAngle = controller.steerInput;
var sgn = Mathf.Sign(steeringTarget - steeringAngle);
var steeringRate = (float)msg.steering_rate * sgn;
steeringAngle += steeringRate * dt;
// to prevent oversteering
if (sgn != steeringTarget - steeringAngle)
{
steeringAngle = steeringTarget;
}
if (!underKeyboardControl)
{
autoSteerAngle = steeringAngle;
autoInputAccel = linearAccel;
}
}));
}
};
}