public void OnRosConnected()
{
if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (System.Action <Ros.VehicleCmd>)((Ros.VehicleCmd msg) =>
{
lastUpdate = Time.time;
var targetLinear = (float)msg.twist_cmd.twist.linear.x;
var targetAngular = (float)msg.twist_cmd.twist.angular.z;
if (!keyboard)
{
var linMag = Mathf.Abs(targetLinear - actualLinVel);
if (actualLinVel < targetLinear && !controller.InReverse)
{
inputAccel = Mathf.Clamp(linMag, 0, constAccel);
}
else if (actualLinVel > targetLinear && !controller.InReverse)
{
inputAccel = -Mathf.Clamp(linMag, 0, constAccel);
}
targetAngVel = -Mathf.Clamp(targetAngular * 0.5f, -constSteer, constSteer);
}
}));
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.Subscribe <Ros.control_command>(APOLLO_CMD_TOPIC, (System.Action <Ros.control_command>)(msg =>
{
lastUpdate = 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 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 (!keyboard)
{
targetAngVel = steeringAngle;
inputAccel = linearAccel;
}
}));
}
}
public void OnRosConnected()
{
Bridge.Subscribe <Ros.control_command>(VehicleInputController.APOLLO_CMD_TOPIC, msg =>
{
Wheel.autonomousBehavior = SteerWheelAutonomousFeedbackBehavior.OutputOnly;
});
}
public void OnRosConnected()
{
Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (System.Action <Ros.VehicleCmd>)((Ros.VehicleCmd msg) =>
{
var targetLinear = msg.twist_cmd.twist.linear;
var targetAngular = msg.twist_cmd.twist.angular;
if (!keyboard)
{
targetAngVel = (float)targetAngular.z;
targetLinVel = (float)targetLinear.x;
}
}));
}
public void OnRosConnected()
{
Bridge.Subscribe <Ros.Vector3>(ResetTopic, msg =>
{
var position = GpsDevice.GetPosition(msg.x, msg.y);
int mask = ~(1 << 8); // car layer
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);
transform.SetPositionAndRotation(position, rotation);
});
}
public void OnRosConnected()
{
Bridge.Subscribe <Ros.Vector3>(ResetTopic, msg =>
{
var position = GpsDevice.GetPosition(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 OnRosConnected()
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.AddPublisher <Ros.Detection3DArray>(objects3DTopicName);
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Subscribe <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 OnRosConnected()
{
if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Subscribe(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.Subscribe <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.Subscribe <Ros.TwistStamped>(LANEFOLLOWING_CMD_TOPIC, (System.Action <Ros.TwistStamped>)(msg =>
{
lastAutoUpdate = Time.time;
autoSteerAngle = (float)msg.twist.angular.x;
}));
seq = 0;
Bridge.AddPublisher <Ros.TwistStamped>(SIMULATOR_CMD_TOPIC);
}
}
public void OnRosConnected()
{
Bridge.Subscribe(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.Subscribe(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);
});
string override_topic;
if (Bridge.Version == 1)
{
Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
Bridge.AddPublisher <BoolStamped>(override_topic);
Bridge.Subscribe <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;
Bridge.Publish(topic, stamp);
FirstConnection = false;
}
}
public void OnRosConnected()
{
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.AddPublisher <Ros.Detection2DArray>(objects2DTopicName);
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Subscribe <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 OnRosConnected()
{
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 = ""
});
});
// tugbot
Bridge.Subscribe(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.Subscribe(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.Subscribe(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);
});
string override_topic;
if (Bridge.Version == 1)
{
Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
Bridge.AddPublisher <BoolStamped>(override_topic);
Bridge.Subscribe <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;
Bridge.Publish(topic, stamp);
FirstConnection = false;
}
ManualControl = true;
}
