// this gets called when we're connected to the master
async void Start()
{
Uri masterUri = new Uri("http://141.3.59.5:11311");
Uri myUri = RosClient.TryGetCallerUriFor(masterUri, 7635) ?? RosClient.TryGetCallerUri(7635);
connectionClient = await RosClient.CreateAsync(masterUri, "/iviz_test", myUri);
scenePublisher = new RosChannelWriter <PlanningScene> {
LatchingEnabled = true
};
await scenePublisher.StartAsync(connectionClient, "/move_group/monitored_planning_scene");
//Logger.LogDebug = Debug.Log;
Logger.Log = Debug.Log;
Logger.LogError = Debug.LogWarning;
Debug.Log($"{this}: Connected!");
while (true)
{
if (this == null)
{
return;
}
// keep checking whether the moveit_test node is on
const string trajectoryService = "/moveit_test/calculate_trajectory";
var systemState = await connectionClient.GetSystemStateAsync();
bool hasService = systemState.Services.Any(service => service.Topic == trajectoryService);
if (hasService)
{
break;
}
Debug.LogWarning($"{this}: Service not detected! Retrying...");
await Task.Delay(2000);
}
Debug.Log($"{this}: Service found. Starting robot and listeners.");
// generate two robots: 'robot' for the real robot, and 'planRobot' for the plan
await GenerateRobotAsync();
// start listening to the joints topic
// these joints apply only for 'robot'.
jointsListener = new RosChannelReader <JointState>();
await jointsListener.StartAsync(connectionClient, "/joint_states");
// here we create a new collision object for the scene
PlanningScene scene = new PlanningScene();
scene.Name = "(noname+)"; // taken from rviz
scene.World.CollisionObjects = new[]
static async Task MainImpl(string[] args)
{
Console.WriteLine("** Starting Iviz.ModelService...");
Uri?masterUri = RosClient.EnvironmentMasterUri;
if (masterUri is null)
{
Console.Error.WriteLine("EE Fatal error: Failed to determine master uri. " +
"Try setting ROS_MASTER_URI to the address of the master.");
return;
}
Uri myUri = RosClient.TryGetCallerUriFor(masterUri) ?? RosClient.TryGetCallerUri();
await using RosClient client = await RosClient.CreateAsync(masterUri, "iviz_model_service", myUri);
bool enableFileSchema;
if (args.Length != 0 && args[0] == "--enable-file-schema")
{
enableFileSchema = true;
Console.Error.WriteLine("WW Uris starting with 'file://' are now accepted. " +
"This makes all your files available to the outside.");
}
else
{
enableFileSchema = false;
}
string?rosPackagePathExtras = null;
string?extrasPath = await GetPathExtras();
if (extrasPath != null && File.Exists(extrasPath))
{
try
{
rosPackagePathExtras = await File.ReadAllTextAsync(extrasPath);
}
catch (IOException e)
{
Console.WriteLine($"EE Extras file '{extrasPath}' could not be read: {e.Message}");
}
}
using var modelServer = new ModelServer(rosPackagePathExtras, enableFileSchema);
if (modelServer.NumPackages == 0)
{
return;
}
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.ModelServiceName,
GetModelResource.RosServiceType);
await client.AdvertiseServiceAsync <GetModelResource>(ModelServer.ModelServiceName,
modelServer.ModelCallback);
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.TextureServiceName,
GetModelTexture.RosServiceType);
await client.AdvertiseServiceAsync <GetModelTexture>(ModelServer.TextureServiceName,
modelServer.TextureCallback);
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.FileServiceName, GetFile.RosServiceType);
await client.AdvertiseServiceAsync <GetFile>(ModelServer.FileServiceName, modelServer.FileCallback);
Console.WriteLine("** Starting service {0} [{1}]...", ModelServer.SdfServiceName, GetSdf.RosServiceType);
await client.AdvertiseServiceAsync <GetSdf>(ModelServer.SdfServiceName, modelServer.SdfCallback);
Console.WriteLine("** Done.");
Console.WriteLine("** Iviz.ModelService started with " + modelServer.NumPackages + " ROS package path(s).");
Console.WriteLine("** Standing by for requests.");
await WaitForCancel();
Console.WriteLine();
}