protected override void StartRos()
 {
     tfPublisher             = node.CreatePublisher <tf2_msgs.msg.TFMessage>(tfTopic);
     tfMessage               = new tf2_msgs.msg.TFMessage();
     tfMessage.Transforms    = new TransformStamped[1];
     tfMessage.Transforms[0] = new TransformStamped();
 }
    void Start()
    {
        odometryPublisher = node.CreatePublisher <nav_msgs.msg.Odometry>(OdometryTopic);
        tfPublisher       = node.CreatePublisher <tf2_msgs.msg.TFMessage>(TfTopic);

        clock = new rclcs.Clock();

        odometryMsg = new nav_msgs.msg.Odometry();
        odometryMsg.header.frame_id = "/odom";
        odometryMsg.child_frame_id  = "/base_footprint";
        robotPose  = odometryMsg.pose;
        robotTwist = odometryMsg.twist;

        tfMsg = new tf2_msgs.msg.TFMessage();
        tfMsg.transforms_init(1);
        tfTransformStamped = tfMsg.transforms[0];
        tfTransformStamped.header.frame_id = "/odom";
        tfTransformStamped.child_frame_id  = "/base_footprint";

        odometryMsg.pose.covariance  = poseCovarianceDiagonal.CovarianceMatrixFromDiagonal();
        odometryMsg.twist.covariance = twistCovarianceDiagonal.CovarianceMatrixFromDiagonal();

        if (BaseRigidbody == null)
        {
            BaseRigidbody = GetComponentInChildren <Rigidbody>();
        }
    }
Exemple #3
0
 void UpdateTransforms(tf2_msgs.msg.TFMessage msg)
 {
     foreach (var tfTransform in msg.transforms)
     {
         TfFrame tfFrame;
         tfFrameDict.TryGetValue(tfTransform.child_frame_id, out tfFrame);
         if (tfFrame != null)
         {
             tfFrame.TargetPosition = tfTransform.transform.translation.Ros2Unity();
             tfFrame.TargetRotation = tfTransform.transform.rotation.Ros2Unity();
         }
     }
 }
Exemple #4
0
    private tf2_msgs.msg.TFMessage CreateTfMsg()
    {
        var msg = new tf2_msgs.msg.TFMessage
        {
            Transforms = new geometry_msgs.msg.TransformStamped[1],
        };

        var transformStamped = new geometry_msgs.msg.TransformStamped();

        transformStamped.Header.Frame_id = "/odom";
        transformStamped.Child_frame_id  = "/base_footprint";
        msg.Transforms[0] = transformStamped;

        return(msg);
    }
    private tf2_msgs.msg.TFMessage CreateTfMsg()
    {
        var msg = new tf2_msgs.msg.TFMessage
        {
            Transforms = new geometry_msgs.msg.TransformStamped[1],
        };

        var transformStamped = new geometry_msgs.msg.TransformStamped();

        transformStamped.Header.Frame_id = ParentFrame.name;
        transformStamped.Child_frame_id  = ChildFrame.name;
        msg.Transforms[0] = transformStamped;

        return(msg);
    }
Exemple #6
0
    //private List<double> poseCovarianceDiagonal = new List<double> { 0.001d, 0.001d, 0.001d, 0.001d, 0.001d, 0.03d };
    //private List<double> twistCovarianceDiagonal = new List<double> { 0.001d, 0.001d, 0.001d, 0.001d, 0.001d, 0.03d };

    protected override void StartRos()
    {
        odometryMsg       = CreateOdometryMsg();
        odometryPublisher = node.CreatePublisher <nav_msgs.msg.Odometry>(OdometryTopic);

        tfMsg       = CreateTfMsg();
        tfPublisher = node.CreatePublisher <tf2_msgs.msg.TFMessage>("tf");

        if (BaseRigidbody == null)
        {
            BaseRigidbody = GetComponent <Rigidbody>();
        }

        StartCoroutine("PublishOdometry");
        if (PublishTf)
        {
            StartCoroutine("PublishTfRoutine");
        }
    }
    protected override void StartRos()
    {
        if (ParentFrame == null)
        {
            ParentFrame = transform;
        }

        if (ParentFrame != null && ChildFrame != null)
        {
            tfMsg       = CreateTfMsg();
            tfPublisher = node.CreatePublisher <tf2_msgs.msg.TFMessage>("tf");

            StartCoroutine("PublishTransform");
        }
        else
        {
            Debug.LogWarning("Tf publisher needs both child and parent frame!");
        }
    }