Esempio n. 1
0
    public void Publish()
    {
        MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints();

        sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
        sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;
        sourceDestinationMessage.joint_02 = jointArticulationBodies[2].xDrive.target;
        sourceDestinationMessage.joint_03 = jointArticulationBodies[3].xDrive.target;
        sourceDestinationMessage.joint_04 = jointArticulationBodies[4].xDrive.target;
        sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;

        // Pick Pose
        sourceDestinationMessage.pick_pose = new MPose
        {
            position    = target.transform.position.To <FLU>(),
            orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
        };

        // Place Pose
        sourceDestinationMessage.place_pose = new MPose
        {
            position    = targetPlacement.transform.position.To <FLU>(),
            orientation = pickOrientation.To <FLU>()
        };

        // Finally send the message to server_endpoint.py running in ROS
        ros.Send(topicName, sourceDestinationMessage);
    }
    void parseXYZ(ROSConnection rs, string filePath)
    {
        string[] lines = File.ReadAllLines(filePath);
        double[] X     = new double[lines.Length];
        double[] Y     = new double[lines.Length];
        double[] Z     = new double[lines.Length];
        int      i     = 0;

        foreach (string line in lines)
        {
            var words = line.Split(' ');
            X[i] = (double.Parse(words[0]) / 1000);
            Y[i] = (double.Parse(words[1]) / 1000);
            Z[i] = (double.Parse(words[2]) / 1000);
            i++;
        }
        RosMessageTypes.ROS.XYZcloud Cloud = new RosMessageTypes.ROS.XYZcloud();
        Cloud.X = X;
        Cloud.Y = Y;
        Cloud.Z = Z;

        if (DisplayPointCloud == true)
        {
            ApplyToParticleSystem(X, Y, Z);
        }
        else
        {
            var ps = GetComponent <ParticleSystem>();
            ps.Stop();
        }

        rs.Send("/unity/XYZCloud", Cloud);
    }
Esempio n. 3
0
        // #################################  Getters ############################################

        // void getPosOri(ArticulationBody link)
        // {
        //     Transform data = link.transform;
        //     // //print(data.eulerAngles);
        //     // print(data.position);
        //     return
        // }

        // void getEEinfo()
        // {

        // }


        // ################################# Publishers #################################

        void publishStates()
        {
            timeElapsed += Time.deltaTime;

            if (timeElapsed > publishMessageFrequency)
            {
                Transform grasp_target = articulationChain[ee_index].transform;
                Transform ee_info      = articulationChain[ee_index - 1].transform;
                PosRot    eePosRot     = new PosRot(
                    grasp_target.position.x,
                    grasp_target.position.y,
                    grasp_target.position.z,
                    ee_info.rotation.x,
                    ee_info.rotation.y,
                    ee_info.rotation.z,
                    ee_info.rotation.w
                    );
                print(ee_info.rotation);
                print(ee_info.eulerAngles);
                print(grasp_target.position);
                // Finally send the message to server_endpoint.py running in ROS
                ros.Send(topicName, eePosRot);

                timeElapsed = 0;
            }
        }
Esempio n. 4
0
    /// <summary>
    ///     Publish the robot's current joint configuration, the target object's
    ///     position and rotation, and the target placement for the target object's
    ///     position and rotation.
    ///
    ///     Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up
    /// </summary>
    public void PublishJoints()
    {
        MoverServiceRequest request = new MoverServiceRequest
        {
            joints_input =
            {
                joint_00 = jointArticulationBodies[0].xDrive.target,
                joint_01 = jointArticulationBodies[1].xDrive.target,
                joint_02 = jointArticulationBodies[2].xDrive.target,
                joint_03 = jointArticulationBodies[3].xDrive.target,
                joint_04 = jointArticulationBodies[4].xDrive.target,
                joint_05 = jointArticulationBodies[5].xDrive.target
            },
            pick_pose = new Pose
            {
                position = (target.transform.position + PICK_POSE_OFFSET).To <FLU>(),
                // The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
                orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
            },
            place_pose = new Pose
            {
                position    = (targetPlacement.transform.position + PICK_POSE_OFFSET).To <FLU>(),
                orientation = pickOrientation.To <FLU>()
            }
        };

        ros.Send(rosJointPublishTopicName, request);
    }
    public void Publish()
    {
        Debug.Log("Detection made!");
        var msg = new RosMessageTypes.Std.Bool(true);

        ros.Send(topicName, msg);
    }
    /// <summary>
    ///
    /// </summary>
    void Start()
    {
        RosMessageTypes.Geometry.Pose[] frame_message    = new RosMessageTypes.Geometry.Pose[Arms.Length];
        RosMessageTypes.Geometry.Pose[] position_message = new RosMessageTypes.Geometry.Pose[Controllers.Length];
        // Get ROS connection static instance
        ros = ROSConnection.instance;
        parseXYZ(ros, Path + fileName);
        int i = 0;

        joint_topics      = new string[Arms.Length];
        frame_topics      = new string[Arms.Length];
        controller_topics = new string[Controllers.Length];
        foreach (GameObject _controller in Controllers)
        {
            controller_topics[i] = "/unity/" + _controller.name + "/pose/";
            position_message[i]  = new RosMessageTypes.Geometry.Pose();
            i++;
        }
        i = 0;
        foreach (GameObject arm in Arms)
        {
            frame_message[i] = new RosMessageTypes.Geometry.Pose();
            joint_topics[i]  = "/unity/" + arm.name + "/joint_state/";
            frame_topics[i]  = "/unity/" + arm.name + "/base_frame/";

            if (arm.name == "ECM")
            {
                i_ECM          = i;
                Base_Frames[i] = GameObject.Find("ecm_setup_base_link");
            }
            else if (arm.name == "PSM1")
            {
                i_PSM1         = i;
                Base_Frames[i] = GameObject.Find(arm.name + "_psm_base_link");
            }
            else if (arm.name == "PSM2")
            {
                i_PSM2         = i;
                Base_Frames[i] = GameObject.Find(arm.name + "_psm_base_link");
            }
            i++;
        }

        for (i = 0; i < Base_Frames.Length; i++)
        {
            frame_message[i].position    = Base_Frames[i].transform.position.To <FLU>();
            frame_message[i].orientation = Base_Frames[i].transform.rotation.To <FLU>();
        }


        i = 0;


        for (i = 0; i < frame_message.Length; i++)
        {
            ros.Send(frame_topics[i], frame_message[i]);
        }
    }
    public void Publish()
    {
        Vector3  mpos     = mobileBase.transform.position;
        Vector3  tpos     = targetCube.transform.position;
        float    angle    = Mathf.Atan2(tpos.z - mpos.z, tpos.x - mpos.x);
        BasePose basePose = new BasePose(tpos.x, tpos.y, tpos.z, angle);

        ros.Send(topicName, basePose);
    }
        // ####################################    Setters ######################################
        // This is to test when joint positions are broadcast
        void broadcastJointPositions()
        {
            DateTime foo      = DateTime.Now;
            long     unixTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();

            JointPositions cmd = new JointPositions(
                jointSliderVals[1], // 1 - shoulder
                jointSliderVals[2], // 2 - upper arm
                jointSliderVals[3], // 3 - forearm
                jointSliderVals[4], // 4 - wrist 1
                jointSliderVals[5], // 5 - wrist 2
                jointSliderVals[6], // 6 - wrist 3
                0.0F,               // for the gripper
                unixTime            // unix time so we can ignore commands which are sitting in ROS' pipes but from prev runs
                );

            // Finally send the message to server_endpoint.py running in ROS
            ros.Send("joint_commands", cmd);
        }
    public void Publish()
    {
        RosMessageTypes.Geometry.Pose[] frame_message    = new RosMessageTypes.Geometry.Pose[Arms.Length];
        RosMessageTypes.Geometry.Pose[] position_message = new RosMessageTypes.Geometry.Pose[Controllers.Length];
        int i = 0;

        foreach (GameObject arm in Arms)
        {
            frame_message[i] = new RosMessageTypes.Geometry.Pose();
            i++;
        }
        i = 0;


        for (i = 0; i < 3; i++)
        {
            frame_message[i].position    = Base_Frames[i].transform.position.To <FLU>();
            frame_message[i].orientation = Base_Frames[i].transform.rotation.To <FLU>();
        }

        i = 0;
        foreach (GameObject _controller in Controllers)
        {
            position_message[i]             = new RosMessageTypes.Geometry.Pose();
            position_message[i].position    = Base_Frames[i].transform.InverseTransformPoint(_controller.transform.position).To <FLU>();
            position_message[i].orientation = Relative(Base_Frames[i].transform.rotation, _controller.transform.rotation).To <FLU>();
            i++;
        }

        for (i = 0; i < controller_topics.Length; i++)
        {
            ros.Send(controller_topics[i], position_message[i]);
        }

        for (i = 0; i < frame_message.Length; i++)
        {
            ros.Send(frame_topics[i], frame_message[i]);
        }
    }
Esempio n. 10
0
    public void Update()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed < publishMessageFrequency)
        {
            return;
        }

        ArmPose armPose = arm.GetArmPose();

        ros.Send(topicName, armPose);

        timeElapsed = 0;
    }
    private void Update()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed < publishMessageFrequency)
        {
            return;
        }

        var pos  = arm.worldJoint.transform.InverseTransformPoint(releasePose.transform.position);
        var pose = new RosMessageTypes.Geometry.Pose(
            new RosMessageTypes.Geometry.Point(pos.x, pos.y, pos.z),
            new RosMessageTypes.Geometry.Quaternion());

        ros.Send(topicName, pose);

        timeElapsed = 0;
    }
Esempio n. 12
0
    private void Update()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed < publishMessageFrequency)
        {
            return;
        }

        Transform tf       = mobileBase.transform;
        Vector3   pos      = tf.position;
        Vector3   pos_d    = pos + tf.forward.normalized;
        float     angle    = Mathf.Atan2(pos_d.z - pos.z, pos_d.x - pos.x);
        BasePose  basePose = new BasePose(pos.x, pos.y, pos.z, angle);

        ros.Send(topicName, basePose);

        timeElapsed = 0;
    }
    private void Update()
    {
        timeElapsed += Time.deltaTime;

        if (timeElapsed > publishMessageFrequency)
        {
            cube.transform.rotation = Random.rotation;

            MPosRot cubePos = new MPosRot(
                cube.transform.position.x,
                cube.transform.position.y,
                cube.transform.position.z,
                cube.transform.rotation.x,
                cube.transform.rotation.y,
                cube.transform.rotation.z,
                cube.transform.rotation.w
                );

            // Finally send the message to server_endpoint.py running in ROS
            ros.Send(topicName, cubePos);

            timeElapsed = 0;
        }
    }
Esempio n. 14
0
    public void Publish()
    {
        NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints();

        sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
        sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;
        sourceDestinationMessage.joint_02 = jointArticulationBodies[2].xDrive.target;
        sourceDestinationMessage.joint_03 = jointArticulationBodies[3].xDrive.target;
        sourceDestinationMessage.joint_04 = jointArticulationBodies[4].xDrive.target;
        sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;

        // Pick Pose
        sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose
        {
            position = new Point(
                target.transform.position.z,
                -target.transform.position.x,
                target.transform.position.y
                ),
            orientation = pickOrientation
        };

        // Place Pose
        sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose
        {
            position = new Point(
                targetPlacement.transform.position.z,
                -targetPlacement.transform.position.x,
                targetPlacement.transform.position.y
                ),
            orientation = pickOrientation
        };

        // Finally send the message to server_endpoint.py running in ROS
        ros.Send(topicName, sourceDestinationMessage);
    }
    /// <summary>
    /// Publish the joint states from the current robot pose and
    /// send to ROS so that it can be used for motion planning
    /// </summary>
    public void PublishJointStates()
    {
        int numRobotJoints = jointArticulationBodies.Count;

        double[] jointPositions  = new double[numRobotJoints];
        double[] jointVelocities = new double[numRobotJoints];
        double[] jointEfforts    = new double[numRobotJoints];
        string[] jointNames      = new string[numRobotJoints];

        jointPositionIndex.Keys.CopyTo(jointNames, 0);

        int c = 0;

        foreach (ArticulationBody body in jointArticulationBodies)
        {
            jointPositions[c]  = body.jointPosition[0];
            jointVelocities[c] = body.jointVelocity[0];
            jointEfforts[c]    = body.jointFriction;
            c += 1;
        }

        RosMessageTypes.Std.Header header = new RosMessageTypes.Std.Header();
        header.stamp = now();
        // Pick Pose
        RosMessageTypes.Sensor.JointState jointState = new RosMessageTypes.Sensor.JointState
        {
            header   = header,
            name     = jointNames,
            position = jointPositions,
            velocity = jointVelocities,
            effort   = jointEfforts
        };

        // Finally send the message to server_endpoint.py running in ROS
        ros.Send(jointStateTopicName, jointState);
    }
 void Start()
 {
     ros.Send(topicName, new RosMessageTypes.Std.Int32(1));
 }
Esempio n. 17
0
    public void Publish(bool status, int id)
    {
        var msg = new TrajectoryStatus(status, id);

        ros.Send(topicName, msg);
    }