protected virtual void _gazebo_client_listener(ServiceStub context, ClientServiceListenerEventType ev, object parameter)
{
if (ev == ClientServiceListenerEventType.ClientClosed || ev == ClientServiceListenerEventType.ServicePathReleased)
{
lock (this)
{
if (ev == ClientServiceListenerEventType.ServicePathReleased)
{
if (((string)parameter) != model_rr_path)
{
return;
}
}
connected = false;
gazebo_server = null;
gazebo_robot = null;
gazebo_controller = null;
gazebo_controller_position = null;
gazebo_controller_position_command = null;
gazebo_controller_velocity = null;
gazebo_controller_velocity_command = null;
}
Console.WriteLine("Connection to gazebo lost");
}
}
protected virtual async Task _connect_gazebo()
{
try
{
org.gazebosim.gazebo.Server server;
org.gazebosim.gazebo.Model model;
org.gazebosim.gazebo.JointController controller;
Wire <Dictionary <string, double> > .WireConnection controller_position;
Wire <Dictionary <string, double> > .WireConnection controller_position_command;
Wire <Dictionary <string, double> > .WireConnection controller_velocity;
Wire <Dictionary <string, double> > .WireConnection controller_velocity_command;
org.gazebosim.gazebo.Server old_server;
lock (this)
{
if (connected || connecting)
{
return;
}
connecting = true;
connected = false;
old_server = gazebo_server;
gazebo_server = null;
gazebo_robot = null;
gazebo_controller = null;
gazebo_controller_position = null;
gazebo_controller_position_command = null;
gazebo_controller_velocity = null;
gazebo_controller_velocity_command = null;
}
try
{
if (old_server != null)
{
RobotRaconteurNode.s.AsyncDisconnectService(old_server).ContinueWith(c => { var ignored = c.Exception; },
TaskContinuationOptions.OnlyOnFaulted |
TaskContinuationOptions.ExecuteSynchronously);
}
}
catch (Exception) { }
Console.WriteLine($"Begin connect to gazebo with url {gazebo_url} and model {gazebo_model_name}");
server = (org.gazebosim.gazebo.Server) await RobotRaconteurNode.s.AsyncConnectService(gazebo_url, null, null, _gazebo_client_listener, null);
var a_server = (org.gazebosim.gazebo.async_Server)server;
var w_names = await a_server.async_get_world_names();
var w = await a_server.async_get_worlds(w_names[0]);
var a_w = (org.gazebosim.gazebo.async_World)w;
model = await a_w.async_get_models(gazebo_model_name);
model_rr_path = RobotRaconteurNode.s.GetObjectServicePath(model);
var a_model = (org.gazebosim.gazebo.async_Model)model;
try
{
await a_model.async_destroy_joint_controller();
}
catch (Exception) { }
try
{
await a_model.async_destroy_kinematic_joint_controller();
}
catch (Exception) { }
await a_model.async_create_kinematic_joint_controller();
controller = await a_model.async_get_kinematic_joint_controller();
var a_controller = (org.gazebosim.gazebo.async_JointController)controller;
foreach (var joint_name in _joint_names)
{
await a_controller.async_add_joint(joint_name);
}
controller_position = await controller.joint_position.AsyncConnect();
controller_position_command = await controller.joint_position_command.AsyncConnect();
controller_velocity = await controller.joint_velocity.AsyncConnect();
controller_velocity_command = await controller.joint_velocity_command.AsyncConnect();
lock (this)
{
gazebo_server = server;
gazebo_robot = model;
gazebo_controller = controller;
gazebo_controller_position = controller_position;
gazebo_controller_position_command = controller_position_command;
gazebo_controller_velocity = controller_velocity;
gazebo_controller_velocity_command = controller_velocity_command;
connected = true;
connecting = false;
}
controller_position.WireValueChanged += _on_joint_position;
controller_velocity.WireValueChanged += _on_joint_velocity;
Console.WriteLine("Connected to gazebo");
}
catch (Exception e)
{
lock (this)
{
connected = false;
}
Console.WriteLine("Error connection to gazebo server: " + e.ToString());
//TODO: send to event log
// Backoff to prevent flooding gazebo
await Task.Delay(500);
lock (this)
{
gazebo_server = null;
gazebo_robot = null;
gazebo_controller = null;
gazebo_controller_position = null;
gazebo_controller_position_command = null;
gazebo_controller_velocity = null;
gazebo_controller_velocity_command = null;
connecting = false;
}
}
}
