static int Main(string[] args)
        {
            bool   shouldShowHelp    = false;
            string robot_info_file   = null;
            string robot_name        = null;
            bool   wait_signal       = false;
            bool   electric_gripper  = false;
            bool   vacuum_gripper    = false;
            string gripper_info_file = null;
            string gripper_name      = null;

            var options = new OptionSet {
                { "robot-info-file=", "the robot info YAML file", n => robot_info_file = n },
                { "robot-name=", "override the robot device name", n => robot_name = n },
                { "h|help", "show this message and exit", h => shouldShowHelp = h != null },
                { "electric-gripper", "rethink electric gripper is attached", n => electric_gripper = n != null },
                { "vacuum-gripper", "rethink vacuum gripper is attached", n => vacuum_gripper = n != null },
                { "gripper-info-file=", "gripper info file", n => gripper_info_file = n },
                { "gripper-name=", "override the gripper device name", n => gripper_name = n },
                { "wait-signal", "wait for POSIX sigint or sigkill to exit", n => wait_signal = n != null }
            };

            List <string> extra;

            try
            {
                // parse the command line
                extra = options.Parse(args);
            }
            catch (OptionException e)
            {
                // output some error message
                Console.Write("SawyerRobotRaconteurDriver: ");
                Console.WriteLine(e.Message);
                Console.WriteLine("Try `SawyerRobotRaconteurDriver --help' for more information.");
                return(1);
            }

            if (shouldShowHelp)
            {
                Console.WriteLine("Usage: SawyerRobotRaconteurDriver [Options+]");
                Console.WriteLine();
                Console.WriteLine("Options:");
                options.WriteOptionDescriptions(Console.Out);
                return(0);
            }

            if (robot_info_file == null)
            {
                Console.WriteLine("error: robot-info-file must be specified");
                return(1);
            }

            if (vacuum_gripper && electric_gripper)
            {
                throw new ArgumentException("--vacuum-gripper and --electric-gripper are mutually exclusive");
            }

            Tuple <RobotInfo, LocalIdentifierLocks> robot_info = null;
            Tuple <ToolInfo, LocalIdentifierLocks>  tool_info  = null;
            SawyerRobot    robot   = null;
            ISawyerGripper gripper = null;


            try
            {
                robot_info = RobotInfoParser.LoadRobotInfoYamlWithIdentifierLocks(robot_info_file, robot_name);

                ros_csharp_interop.ros_csharp_interop.init_ros(args, "sawyer_robotraconteur_driver", false);

                if (electric_gripper || vacuum_gripper)
                {
                    tool_info = ToolInfoParser.LoadToolInfoYamlWithIdentifierLocks(gripper_info_file, gripper_name);
                    tool_info.Item1.device_info.parent_device      = robot_info.Item1.device_info.device;
                    tool_info.Item1.device_info.device_origin_pose = new NamedPose
                    {
                        parent_frame = new Identifier {
                            name = "right_hand", uuid = new com.robotraconteur.uuid.UUID
                            {
                                uuid_bytes = new byte[16]
                            }
                        },
                        pose = new Pose {
                            orientation = new Quaternion {
                                w = 1
                            }
                        }
                    };
                }

                robot = new SawyerRobot(robot_info.Item1, "");
                if (electric_gripper)
                {
                    gripper = new SawyerElectricGripper(tool_info.Item1, "right_gripper", "");
                    gripper._start_tool();
                }
                else if (vacuum_gripper)
                {
                    gripper = new SawyerVacuumGripper(tool_info.Item1, "right_vacuum_gripper", "");
                    gripper._start_tool();
                }

                robot._start_robot();
                using (var node_setup = new ServerNodeSetup("sawyer_robot", 58653, args))
                {
                    var robot_service_ctx = RobotRaconteurNode.s.RegisterService("robot", "com.robotraconteur.robotics.robot", robot);
                    robot_service_ctx.SetServiceAttributes(AttributesUtil.GetDefaultServiceAtributesFromDeviceInfo(robot_info.Item1.device_info));
                    if (gripper != null)
                    {
                        var tool_service_ctx = RobotRaconteurNode.s.RegisterService("gripper", "com.robotraconteur.robotics.tool", gripper);
                        tool_service_ctx.SetServiceAttributes(AttributesUtil.GetDefaultServiceAtributesFromDeviceInfo(tool_info.Item1.device_info));
                    }

                    if (!wait_signal)
                    {
                        Console.WriteLine("Press enter to exit");
                        Console.ReadKey();
                    }
                    else
                    {
                        UnixSignal[] signals = new UnixSignal[] {
                            new UnixSignal(Mono.Unix.Native.Signum.SIGINT),
                            new UnixSignal(Mono.Unix.Native.Signum.SIGTERM),
                        };

                        Console.WriteLine("Press Ctrl-C to exit");
                        // block until a SIGINT or SIGTERM signal is generated.
                        int which = UnixSignal.WaitAny(signals, -1);

                        Console.WriteLine("Got a {0} signal, exiting", signals[which].Signum);
                    }
                }
            }
            finally
            {
                robot_info?.Item2?.Dispose();
                tool_info?.Item2?.Dispose();
                robot?.Dispose();
                gripper?.Dispose();
            }

            return(0);
        }
Ejemplo n.º 2
0
 internal SawyerHomingTask(SawyerRobot parent)
 {
     this.parent = parent;
 }