public override IObservable <Space> Process(IObservable <Space> source)
{
return(Observable.Defer(() =>
{
var handlers = collisionHandlers.ToDictionary(h => new CollisionHandlerKey(h.Material1, h.Material2));
var contacts = new ContactGeom[MaxContacts];
var collisionGroup = new JointGroup();
return source.Do(space =>
{
collisionGroup.Clear();
space.Collide((g1, g2) =>
{
var numContacts = Geom.Collide(g1, g2, contacts);
if (numContacts == 0)
{
return;
}
var body1 = g1.Body;
var body2 = g2.Body;
var metadata1 = g1.Tag as GeomMetadata;
var metadata2 = g2.Tag as GeomMetadata;
if (metadata1 != null)
{
metadata1.OnCollision(g1, g2, contacts, numContacts);
}
if (metadata2 != null)
{
metadata2.OnCollision(g2, g1, contacts, numContacts);
}
CollisionHandler collisionHandler;
if (metadata1 != null && metadata2 != null &&
handlers.TryGetValue(new CollisionHandlerKey(metadata1.Material, metadata2.Material), out collisionHandler))
{
if (body1 != null || body2 != null)
{
if (ExcludeConnected && body1 != null && body2 != null &&
Body.AreConnectedExcluding(body1, body2, JointType.Contact))
{
return;
}
var collisionSurface = collisionHandler.CollisionSurface;
var world = body1 != null ? body1.World : body2.World;
for (int i = 0; i < numContacts; i++)
{
var contactInfo = new ContactInfo();
contactInfo.Geometry = contacts[i];
contactInfo.Surface = collisionSurface;
var contact = new Contact(world, contactInfo, collisionGroup);
contact.Attach(body1, body2);
}
}
}
});
}).Finally(collisionGroup.Clear);
}));
}
public MoveItClient()
{
rosSocket = new RosSocket(new RosSharp.RosBridgeClient.Protocols.WebSocketNetProtocol(uri), RosSocket.SerializerEnum.Newtonsoft_JSON);
/*
* TRAJECTORY SETTING
*/
jointGroup = new JointGroup
{
name = "panda_arm_hand",
jointNames = new string[] { "panda_joint1", "panda_joint2", "panda_joint3",
"panda_joint4", "panda_joint5", "panda_joint6",
"panda_joint7", "panda_finger_joint1", "panda_finger_joint2" }
};
/*
* ENVIRONMENT SETTING
*/
AddCollisionObjects("Box", boxPosition[0], boxPosition[1], boxPosition[2]);
AddCollisionObjects("Screen", screenPosition[0], screenPosition[1], screenPosition[2]);
AddCollisionObjects("Inducter", inducterPosition[0], inducterPosition[1], inducterPosition[2]);
AddCollisionObjects("Arch", archXPosition, inducterPosition[1], inducterPosition[2]);
AddCollisionObjects("Stick", archXPosition, inducterPosition[1], inducterPosition[2]);
/*
* INITIALIZE TRAJECTORY DICTIONARY
*/
trajectoryDict = new Dictionary <int, Dictionary <int, object> >();
/*
* INITIALIZE CLIENT
*/
moveGroupActionClient = new MoveGroupActionClient(actionName, jointGroup, plannerID, rosSocket);
moveGroupActionClient.PlanOnly = false;
moveGroupActionClient.Initialize();
/*
* MOVE TO INITIAL POSITION
*/
MoveToInitialPosition();
/*
* PLANNER PARAMETRIZATION
*/
//plannerID = "ChompDefault";
//SetPlannerParameters();
//GetPlannerParameters();
}
internal Wrapper(JointGroup joints)
{
this.value = joints;
}
