Ejemplo n.º 1
0
 public OdometryMsg(HeaderMsg header, string child_frame_id, PoseWithCovarianceMsg pose, TwistWithCovarianceMsg twist)
 {
     _header         = header;
     _child_frame_id = child_frame_id;
     _pose           = pose;
     _twist          = twist;
 }
Ejemplo n.º 2
0
    void Update()
    {
        ros.Render();

        if (!_running)
        {
            return;
        }

        timer -= Time.deltaTime;

        if (timer <= 0)
        {
            timer = pollRate;

            PointMsg               point  = new PointMsg(1, 2, 3);
            QuaternionMsg          quat   = new QuaternionMsg(1, 2, 3, 4);
            PoseMsg                pose   = new PoseMsg(point, quat);
            PoseWithCovarianceMsg  posec  = new PoseWithCovarianceMsg(pose);
            Vector3Msg             vec3   = new Vector3Msg(1, 2, 3);
            TwistMsg               twist  = new TwistMsg(vec3, vec3);
            TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
            HeaderMsg              header = new HeaderMsg(1, new TimeMsg(1, 1), "0");

            PoseStampedMsg ps  = new PoseStampedMsg(header, pose);
            PathMsg        msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });

            BoolMsg             boolmsg = new BoolMsg(true);
            StringMsg           str     = new StringMsg("This is a test");
            RegionOfInterestMsg roi     = new RegionOfInterestMsg(0, 1, 2, 3, true);
            CameraInfoMsg       caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);

            _genericPub.PublishData(caminfo);
        }
    }
Ejemplo n.º 3
0
 public override void Deserialize(JSONNode msg)
 {
     _header         = new HeaderMsg(msg["header"]);
     _child_frame_id = msg["child_frame_id"].ToString();
     _pose           = new PoseWithCovarianceMsg(msg["pose"]);
     _twist          = new TwistWithCovarianceMsg(msg["twist"]);
 }
Ejemplo n.º 4
0
 public OdometryMsg(JSONNode msg)
 {
     _header         = new HeaderMsg(msg["header"]);
     _child_frame_id = msg["child_frame_id"];
     _pose           = new PoseWithCovarianceMsg(msg["pose"]);
     _twist          = new TwistWithCovarianceMsg(msg["twist"]);
 }
Ejemplo n.º 5
0
    public override void OverridePositionAndOrientation(Vector3 newPosition, Quaternion newOrientation)
    {
        _rosLogger.PublishData(new StringMsg("Odometry overwritten"));

        //Unity repositioning for the virtual robot.
        transform.SetPositionAndRotation(newPosition, newOrientation);

        //reset initial poses for the odom calculation. This is the equivalent of moving the robot in the physical room.
        // We simply move the local coords.
        InitialPosition = newPosition;
        InitialRotation = newOrientation;

        //Publishing to odo_calib_pose for initial gps position, just like the arlobot controller.
        GeoPointWGS84         wgs84 = newPosition.ToUTM().ToWGS84();
        PoseWithCovarianceMsg pose  = new PoseWithCovarianceMsg(
            new PoseMsg(
                new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
                new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
                ));


        OdometryMsg odometryUpdate = new OdometryMsg(pose);

        _rosOdometryOverride.PublishData(odometryUpdate);
    }
Ejemplo n.º 6
0
 public OdometryMsg(PoseWithCovarianceMsg pose)
 {
     _header         = new HeaderMsg(0, new TimeMsg(0, 0), "0");
     _child_frame_id = "0";
     _pose           = pose;
     _twist          = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(0, 0, 0), new Vector3Msg(0, 0, 0)), new double[36]);
 }
 public DetectedPersonMsg(float confidence, uint detection_id, string modality, PoseWithCovarianceMsg pose)
 {
     _confidence   = confidence;
     _detection_id = detection_id;
     _modality     = modality;
     _pose         = pose;
 }
 public DetectedPersonMsg(JSONNode msg)
 {
     _confidence   = msg["confidence"].AsFloat;
     _detection_id = uint.Parse(msg["detection_id"]);
     _modality     = msg["modality"];
     _pose         = new PoseWithCovarianceMsg(msg["pose"]);
 }
Ejemplo n.º 9
0
    //publishing on the /odom topic
    // the newposition should be the difference between the current position
    //and the InitialPosition of the robot in Unity vec3, effectively simulating the local space coords of odom
    //The geopoint conversion is in here.
    public void PublishOdometryData(Vector3 newPosition, Quaternion newOrientation)
    {
        GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();

        PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
            new PoseMsg(
                new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
                new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
                ));

        OdometryMsg odometryUpdate = new OdometryMsg(pose);
        // _rosOdometry.PublishData(odometryUpdate);
    }
Ejemplo n.º 10
0
    public override void OverridePositionAndOrientation(Vector3 newPosition, Quaternion newOrientation)
    {
        GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();

        PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
            new PoseMsg(
                new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
                new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
                ));

        OdometryMsg odometryOverride = new OdometryMsg(pose);

        _rosOdometryOverride.PublishData(odometryOverride);
    }
Ejemplo n.º 11
0
    //callback functions is called whenever the message is received

    public static void CallBack(ROSBridgeMsg msg)
    {
        GameObject cube_obj = GameObject.Find("Cube");

        if (cube_obj == null)
        {
            Debug.Log("Cube not found!!!");
        }
        else
        {
            OdometryMsg           odom    = (OdometryMsg)msg;
            PoseWithCovarianceMsg posemsg = odom._pose;
            PoseMsg pose = posemsg.GetPose(); // get the pose part of the odometry message

            //move the cube in the scene along with the kinect which is on top of the robot

            cube_obj.transform.position = new Vector3(pose._position.GetX(), pose._position.GetZ(), pose._position.GetY());
            cube_obj.transform.rotation = Quaternion.Euler(-pose._orientation.GetX() * 180.0f / 3.1415f, -pose._orientation.GetZ() * 180.0f / 3.1415f, -pose._orientation.GetY() * 180.0f / 3.1415f);
        }
    }
Ejemplo n.º 12
0
    //Publisher to /robot_gps_pose every interval time
    private IEnumerator SendTransformUpdate(float interval)
    {
        while (true)
        {
            //Add the Noise here. Unity should have the ACTUAL position of the physical world
            // and what it publishes and we see on the webmap is the where it thinks it is.
            GeoPointWGS84 wgs = transform.position.ToUTM().ToWGS84();
            Quaternion    rot = transform.rotation;

            PoseMsg pose = new PoseMsg(new PointMsg(wgs.longitude, wgs.latitude, wgs.altitude),
                                       new QuaternionMsg(rot.x, rot.y, rot.z, rot.w));
            PoseWithCovarianceMsg poseWithCovariance = new PoseWithCovarianceMsg(pose, new double[36]);

            OdometryMsg odometry = new OdometryMsg(poseWithCovariance);
            odometry._pose = poseWithCovariance;
            _rosOdometryGPSPose.PublishData(odometry);

            yield return(new WaitForSeconds(interval));
        }
    }
Ejemplo n.º 13
0
        // Send Odometry to ROS
        private IEnumerator SendOdometryToROS(ROS ros)
        {
            while (Application.isPlaying)
            {
                if (ros.IsConnected())
                {
                    double[] covariance_pose = new double[36];

                    for (int i = 0; i < covariance_pose.Length; i++)
                    {
                        covariance_pose[i] = 0.0f;
                    }

                    HeaderMsg             globalHead = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
                    PoseWithCovarianceMsg pose       = new PoseWithCovarianceMsg(new PoseMsg(new PointMsg(transform.position, true), new QuaternionMsg(transform.rotation, true)),
                                                                                 covariance_pose);
                    TwistWithCovarianceMsg twist = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(agent.velocity, true), new Vector3Msg(0.0f, 0.0f, 0.0f)), covariance_pose);

                    OdometryMsg odometrymsg = new OdometryMsg(globalHead, "odom", pose, twist);
                    ros.Publish(Odometry_pub.GetMessageTopic(), odometrymsg);
                }
                yield return(new WaitForSeconds(rOSFrecuencyOdometry));
            }
        }
 public PoseWithCovarianceStampedMsg(HeaderMsg header, PoseWithCovarianceMsg pose)
 {
     _header = header;
     _pose   = pose;
 }
 public PoseWithCovarianceStampedMsg(JSONNode msg)
 {
     _header = new HeaderMsg(msg ["header"]);
     _pose   = new PoseWithCovarianceMsg(msg["pose"]);
 }