void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.CameraEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
CreateDefaultState();
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.LaserRangeFinderEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
CreateDefaultState();
physics.RaycastProperties raycastProperties = new physics.RaycastProperties();
raycastProperties.StartAngle = -_state.AngularRange / 2.0f;
raycastProperties.EndAngle = _state.AngularRange / 2.0f;
raycastProperties.AngleIncrement = (float)_state.AngularResolution;
raycastProperties.Range = LASER_RANGE;
raycastProperties.OriginPose = new Pose();
_entity.RaycastProperties = raycastProperties;
try
{
_entity.Register(_raycastResults);
}
catch (Exception ex)
{
LogError(ex);
}
// attach handler to raycast results port
Activate(Arbiter.Receive(true, _raycastResults, RaycastResultsHandler));
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
// _entity = (simengine.BumperArrayEntity)ins.Body;
_entity = (PioneerBumperEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
// reinitialize the state
CreateDefaultState();
pxContactSensor.ContactSensor cs = null;
// The simulation bumper service uses a simple heuristic to assign contact sensors, to physics shapes:
// Half the sensors go infront, the other half in the rear. Assume front sensors are first in the list
// In the for loop below we create a lookup table that matches simulation shapes with sensors. When
// the physics engine notifies us with a contact, it provides the shape that came into contact. We need to
// translate that to sensor. In the future we might add an object Context field to shapes to make this easier
for (int i = 0; i < _entity.Shapes.Length; i++)
{
cs = new pxContactSensor.ContactSensor();
cs.Name = _entity.Shapes[i].State.Name;
cs.HardwareIdentifier = i;
_state.Sensors.Add(cs);
_bumperShapeToSensorTable.Add((physics.BoxShape)_entity.Shapes[i], cs);
}
// subscribe to bumper simulation entity for contact notifications
_entity.Subscribe(_contactNotificationPort);
// Activate a handler on the notification port, it will run when contacts occur in simulation
Activate(Arbiter.Receive(false, _contactNotificationPort, PhysicsContactNotificationHandler));
}
void OnInsertEntity(sime.InsertSimulationEntity insert)
{
_entity = insert.Body as FourWheelDriveEntity;
if (_entity != null)
{
_entity.ServiceContract = Contract.Identifier;
if (_entity.ChassisShape != null)
{
_state.DistanceBetweenWheels = _entity.DistanceBetweenWheels;
_state.WheelBase = _entity.WheelBase;
}
}
}
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
InsertEntityNotificationHandler(ins);
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
),
new ConcurrentReceiverGroup()
)
);
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.DifferentialDriveEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
// create default state based on the physics entity
if (_entity.ChassisShape != null)
{
_state.DistanceBetweenWheels = _entity.ChassisShape.BoxState.Dimensions.X;
}
_state.LeftWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
_state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
//SpawnIterator(TestDriveDistanceAndRotateDegrees);
}
void OnFoundEntity(sime.InsertSimulationEntity insert)
{
OnInsertEntity(insert);
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <sime.InsertSimulationEntity>(true, _simNotify, OnInsertEntity),
Arbiter.Receive <sime.DeleteSimulationEntity>(true, _simNotify, OnDeleteEntity)
),
new ConcurrentReceiverGroup()
)
);
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.KukaLBR3Entity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
_state = new arm.ArticulatedArmState();
// use the entity state as our state
_state.Joints = _entity.Joints;
// create dictionary for quick lookup of joints from name
_jointLookup = new Dictionary <string, Joint>();
foreach (Joint j in _state.Joints)
{
_jointLookup.Add(j.State.Name, j);
}
}
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
InsertEntityNotificationHandler(ins);
base.Start();
// Listen on the main port for requests and call the appropriate handler.
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler),
Arbiter.Receive <FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler)
),
new ConcurrentReceiverGroup()
)
);
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.VisualEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
// traverse the entities associated with this entity and build a list of joints
_state.Joints.Clear();
Dictionary <string, simengine.VisualEntity> entities = new Dictionary <string, simengine.VisualEntity>();
FindJointsAndEntities(_entity, entities);
// add a slider for each joint to the UI
WinFormsServicePort.FormInvoke(
delegate()
{
_simulatedBipedMoverUI.AddEntityName(_entity.State.Name, _state.Joints == null || _state.Joints.Count == 0);
_simulatedBipedMoverUI.AddSliders(_state.Joints);
}
);
}
IEnumerator <ITask> OnInsertEntity(sime.InsertSimulationEntity insert)
{
_entity = insert.Body;
var query = new sime.VisualEntity();
query.State.Name = _state.CameraName;
yield return(Arbiter.Choice(
_simPort.Query(query),
success => _camera = success.Entity as Entities.PursuitCamera.PursuitCameraEntity,
failure => LogError("Unable to find camera", failure)
));
if (_camera == null)
{
_entity = null;
yield break;
}
SetCameraProperties();
_camera.Target = _entity;
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.DepthCameraEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
try
{
_entity.Register(_raycastResults);
}
catch (Exception ex)
{
LogError(ex);
}
if (_rayCastQueue == null)
{
_rayCastQueue = new DispatcherQueue(_entity.EntityState.Name + "depthNotifications",
TaskQueue.Dispatcher,
TaskExecutionPolicy.ConstrainQueueDepthDiscardTasks,
1);
// attach handler to raycast results port
_rayCastQueue.Enqueue(Arbiter.ReceiveWithIterator(false, _raycastResults, RaycastResultsHandler));
}
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.CameraEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
_state.CameraDeviceName = _entity.State.Name;
_state.Pose = _entity.State.Pose;
_state.ViewAngle = _entity.ViewAngle;
_state.ImageSize = new Vector2(_entity.ViewSizeX, _entity.ViewSizeY);
// send an initial update to self so we get image data filled in
PostUpdateFrameIfNeeded();
if (_entity.IsRealTimeCamera)
{
if (_entity.UpdateInterval != 0)
{
Activate(Arbiter.Receive(false, TimeoutPort(_entity.UpdateInterval), CheckForUpdate));
}
else
{
Activate(Arbiter.Receive(false, TimeoutPort(1000), CheckForUpdate));
}
}
}
/// <summary>
/// This handler receives an announcement from the simulation engine that
/// contains a pointer to the entity associated with this service.
/// </summary>
/// <param name="ins"></param>
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.IRobotCreate)ins.Body;
_entity.ServiceContract = Contract.Identifier;
_contactPort = new Port <simcommon.EntityContactNotification>();
// add the contact handler
MainPortInterleave.CombineWith(new Interleave(
new TeardownReceiverGroup(
),
new ExclusiveReceiverGroup
(
),
new ConcurrentReceiverGroup
(
Arbiter.Receive <simcommon.EntityContactNotification>(true, _contactPort, ContactHandler)
)
));
_entity.Subscribe(_contactPort);
PartnerType irEntityPartner = Dss.ServiceModel.DsspServiceBase.DsspServiceBase.FindPartner(
new System.Xml.XmlQualifiedName("IREntity", Contract.Identifier), ServiceInfo.PartnerList);
// only add an IR Entity if one was requested
if (irEntityPartner != null)
{
// add an IR sensor, if necessary
_irEntity = null;
foreach (simengine.VisualEntity child in _entity.Children)
{
if (child.GetType() == typeof(IREntity))
{
_irEntity = (IREntity)child;
}
}
if (_irEntity == null)
{
// didn't find on already there so add one
// irentity is on the right front quadrant of the Create facing to the right
_irEntity = new IREntity(_entity.State.Name + "_IR", new Pose(
new Vector3(0.115f, 0.055f, -0.115f),
simengine.TypeConversion.FromXNA(xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float)(Math.PI / 2)))));
_entity.InsertEntity(_irEntity);
// refresh the whole entity
System.Reflection.MethodInfo mInfo = typeof(simengine.SimulationEngine).GetMethod("RefreshEntity",
System.Reflection.BindingFlags.Instance | System.Reflection.BindingFlags.NonPublic | System.Reflection.BindingFlags.Public);
if (mInfo != null)
{
mInfo.Invoke(simengine.SimulationEngine.GlobalInstance, new object[] { _entity });
}
}
}
// start up the polling timer
int timerInterval = (_state.PollingInterval <= 0) ? 200 : _state.PollingInterval;
if ((timerInterval > 0 && timerInterval < 200))
{
timerInterval = 200;
}
StartTimer(timerInterval);
// create default state based on the physics entity
//_state.DistanceBetweenWheels = _entity.ChassisShape.BoxState.Dimensions.X;
//_state.LeftWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
//_state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
}
