Beispiel #1
0
        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");
            }
        }
Beispiel #2
0
        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;
                }
            }
        }