|
|
|
|
|
|
using RosSharp; |
|
|
|
using RosSharp.Control; |
|
|
|
using RosSharp.Urdf.Editor; |
|
|
|
using Unity.EditorCoroutines.Editor; |
|
|
|
using UnityEditor; |
|
|
|
using UnityEngine; |
|
|
|
using Unity.Robotics.ROSTCPConnector; |
|
|
|
|
|
|
{ |
|
|
|
EditorApplication.LockReloadAssemblies(); |
|
|
|
SetupScene(); |
|
|
|
SetupRos(); |
|
|
|
CreateTrajectoryPlannerPubliser(); |
|
|
|
AddRosMessages(); |
|
|
|
ImportRobot(); |
|
|
|
CreateRosConnection(); |
|
|
|
CreateTrajectoryPlannerPublisher(); |
|
|
|
} |
|
|
|
|
|
|
|
void Update() |
|
|
|
|
|
|
private void SetupScene() |
|
|
|
{ |
|
|
|
// Instantiate the table, target, and target placement
|
|
|
|
GameObject table = InstantiatePrefab(tableName); |
|
|
|
GameObject target = InstantiatePrefab(targetName); |
|
|
|
GameObject targetPlacement = InstantiatePrefab(targetPlacementName); |
|
|
|
InstantiatePrefab(tableName); |
|
|
|
InstantiatePrefab(targetName); |
|
|
|
InstantiatePrefab(targetPlacementName); |
|
|
|
|
|
|
|
// Import Niryo One with URDF importer
|
|
|
|
string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); |
|
|
|
ImportRobot(urdfFilepath, ImportSettings.convexDecomposer.unity); |
|
|
|
|
|
|
|
// Adjust robot parameters
|
|
|
|
Controller controller = GameObject.Find(niryoOneName).GetComponent<Controller>(); |
|
|
|
controller.stiffness = controllerStiffness; |
|
|
|
controller.damping = controllerDamping; |
|
|
|
controller.forceLimit = controllerForceLimit; |
|
|
|
controller.speed = controllerSpeed; |
|
|
|
controller.acceleration = controllerAcceleration; |
|
|
|
GameObject.Find(baseLinkName).GetComponent<ArticulationBody>().immovable = true; |
|
|
|
private void SetupRos() |
|
|
|
private void AddRosMessages() |
|
|
|
{ |
|
|
|
if (generateRosMessages) |
|
|
|
{ |
|
|
|
|
|
|
scripts.AddRange(Directory.GetFiles(rosMessagesDirectory, scriptPattern, SearchOption.AllDirectories)); |
|
|
|
scripts.AddRange(Directory.GetFiles(externalScriptsDirectory)); |
|
|
|
RecompileScripts(scripts.ToArray()); |
|
|
|
|
|
|
|
// Create RosConnect
|
|
|
|
GameObject rosConnect = new GameObject(rosConnectName); |
|
|
|
rosConnection = rosConnect.AddComponent<ROSConnection>(); |
|
|
|
rosConnection.rosIPAddress = hostIP; |
|
|
|
rosConnection.rosPort = hostPort; |
|
|
|
rosConnection.overrideUnityIP = overrideUnityIP; |
|
|
|
rosConnection.unityPort = unityPort; |
|
|
|
rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; |
|
|
|
rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; |
|
|
|
private void CreateTrajectoryPlannerPubliser() |
|
|
|
private void CreateTrajectoryPlannerPublisher() |
|
|
|
{ |
|
|
|
GameObject publisher = new GameObject(publisherName); |
|
|
|
dynamic planner = publisher.AddComponent(assembly.GetType(trajectoryPlannerType)); |
|
|
|
|
|
|
return gameObject; |
|
|
|
} |
|
|
|
|
|
|
|
private void ImportRobot(string urdfFilepath, ImportSettings.convexDecomposer convexDecomposer) |
|
|
|
private void CreateRosConnection() |
|
|
|
{ |
|
|
|
// Create RosConnect
|
|
|
|
GameObject rosConnect = new GameObject(rosConnectName); |
|
|
|
rosConnection = rosConnect.AddComponent<ROSConnection>(); |
|
|
|
rosConnection.rosIPAddress = hostIP; |
|
|
|
rosConnection.rosPort = hostPort; |
|
|
|
rosConnection.overrideUnityIP = overrideUnityIP; |
|
|
|
rosConnection.unityPort = unityPort; |
|
|
|
rosConnection.awaitDataMaxRetries = awaitDataMaxRetries; |
|
|
|
rosConnection.awaitDataSleepSeconds = awaitDataSleepSeconds; |
|
|
|
} |
|
|
|
|
|
|
|
private void ImportRobot() |
|
|
|
// Import Niryo One by URDF Importer
|
|
|
|
ImportSettings settings = new ImportSettings |
|
|
|
ImportSettings urdfImportSettings = new ImportSettings |
|
|
|
convexMethod = convexDecomposer, |
|
|
|
convexMethod = ImportSettings.convexDecomposer.unity |
|
|
|
UrdfRobotExtensions.Create(urdfFilepath, settings); |
|
|
|
// Import Niryo One with URDF importer
|
|
|
|
string urdfFilepath = Path.Combine(Application.dataPath, urdfRelativeFilepath); |
|
|
|
// Create is a coroutine that would usually run only in EditMode, so we need to force its execution here
|
|
|
|
var robotImporter = UrdfRobotExtensions.Create(urdfFilepath, urdfImportSettings, false); |
|
|
|
while (robotImporter.MoveNext()) {} |
|
|
|
// Adjust robot parameters
|
|
|
|
Controller controller = GameObject.Find(niryoOneName).GetComponent<Controller>(); |
|
|
|
controller.stiffness = controllerStiffness; |
|
|
|
controller.damping = controllerDamping; |
|
|
|
controller.forceLimit = controllerForceLimit; |
|
|
|
controller.speed = controllerSpeed; |
|
|
|
controller.acceleration = controllerAcceleration; |
|
|
|
GameObject.Find(baseLinkName).GetComponent<ArticulationBody>().immovable = true; |
|
|
|
} |
|
|
|
|
|
|
|
// Credit to https://www.codeproject.com/Tips/715891/Compiling-Csharp-Code-at-Runtime
|
|
|
|