public void physiscsCallback(MessageTypes.Gazebo.GetPhysicsPropertiesResponse rep)
{
//create default SetPhysicsMessage
MessageTypes.Gazebo.SetPhysicsPropertiesRequest newDefault =
new MessageTypes.Gazebo.SetPhysicsPropertiesRequest(
time_step: rep.time_step, max_update_rate:
rep.max_update_rate, gravity: rep.gravity, rep.ode_config);
this.DefaultPhysics = newDefault;
isPhysicsPaused = rep.pause;
}
//sets the current speed of the simulation, this basically allows slowing down and speeding
// it up, this method should not be used to pause the simulation completly
public void setSimulationSpeed(double newSpeed = 0.001)
{
//create a message based on the cached params by cloning it's values
MessageTypes.Gazebo.SetPhysicsPropertiesRequest request =
this.createCloneForPhysics(this.DefaultPhysics);// None;
// setting eerything else back to defaults
request.time_step = newSpeed;
RosSocket.CallService <MessageTypes.Gazebo.SetPhysicsPropertiesRequest,
MessageTypes.Gazebo.SetPhysicsPropertiesResponse>
(ServiceEndpoint, responseHandler, request);
}
//sets the simulations settings back to the default
public void normalPhysics()
{
//note also toggles the time
RosSocket.CallService <MessageTypes.Std.EmptyRequest,
MessageTypes.Std.EmptyResponse>
(UnpauseEndpoint, PauseContinueResponseHandler,
new MessageTypes.Std.EmptyRequest());
//set the default physics
MessageTypes.Gazebo.SetPhysicsPropertiesRequest pauseRequest =
new MessageTypes.Gazebo.SetPhysicsPropertiesRequest();
RosSocket.CallService <MessageTypes.Gazebo.SetPhysicsPropertiesRequest,
MessageTypes.Gazebo.SetPhysicsPropertiesResponse>
(ServiceEndpoint, responseHandler, pauseRequest);
}
//clones the settings provided in new messages that can be modified without changing the currently saved default
private MessageTypes.Gazebo.SetPhysicsPropertiesRequest createCloneForPhysics(MessageTypes.Gazebo.SetPhysicsPropertiesRequest orig)
{
MessageTypes.Gazebo.ODEPhysics clonedOEDPhzsics = new MessageTypes.Gazebo.ODEPhysics(orig.ode_config.auto_disable_bodies,
orig.ode_config.sor_pgs_precon_iters,
orig.ode_config.sor_pgs_iters,
orig.ode_config.sor_pgs_w,
orig.ode_config.sor_pgs_rms_error_tol,
orig.ode_config.contact_surface_layer,
orig.ode_config.contact_max_correcting_vel,
orig.ode_config.cfm,
orig.ode_config.erp,
orig.ode_config.max_contacts);
MessageTypes.Gazebo.SetPhysicsPropertiesRequest clone =
new MessageTypes.Gazebo.SetPhysicsPropertiesRequest(orig.time_step,
orig.max_update_rate, orig.gravity, clonedOEDPhzsics);
return(clone);
}
