void Update()
    {
        Vector3 localVelocity        = BaseRigidbody.transform.InverseTransformDirection(BaseRigidbody.velocity).Unity2Ros();
        Vector3 localAngularVelocity = BaseRigidbody.transform.InverseTransformDirection(BaseRigidbody.angularVelocity).Unity2Ros();

        if (LeftWheelJoint != null && RightWheelJoint != null)
        {
            float leftWheelAngularVelocity  = (localVelocity.x + localAngularVelocity.z * WheelBase / 2.0f) / WheelRadius;
            float rightWheelAngularVelocity = (localVelocity.x - localAngularVelocity.z * WheelBase / 2.0f) / WheelRadius;


            rightWheelAngularPosition += rightWheelAngularVelocity * Time.deltaTime;
            leftWheelAngularPosition  += leftWheelAngularVelocity * Time.deltaTime;

            jointStateMsg.name = new List <string> {
                LeftWheelJoint.JointName, RightWheelJoint.JointName
            };
            jointStateMsg.position = new List <double> {
                leftWheelAngularPosition, rightWheelAngularPosition
            };

            publisher.Publish(jointStateMsg);
        }

        Rclcs.SpinOnce(node, 0.0d);
    }
Beispiel #2
0
        public void ImagePubSub()
        {
            bool callbackTriggered = false;

            var subscription = sub_node.CreateSubscription <sensor_msgs.msg.Image>(
                "test_topic", (received_msg) =>
            {
                callbackTriggered = true;
                Assert.That(received_msg.Data.Length, Is.EqualTo(10));
            });

            var publisher = pub_node.CreatePublisher <sensor_msgs.msg.Image>("test_topic");
            var msg       = new sensor_msgs.msg.Image();

            msg.Data    = new byte[10];
            msg.Data[0] = 1;
            publisher.Publish(msg);

            Rclcs.SpinOnce(sub_node, sub_context, 0.5);

            Assert.That(callbackTriggered, Is.True);

            publisher.Dispose();
            subscription.Dispose();
        }
        public void SubscriptionTriggerCallback()
        {
            bool callbackTriggered = false;

            node.CreateSubscription <std_msgs.msg.Int32>("subscription_test_topic", (msg) => { callbackTriggered = true; });
            publisher.Publish(new std_msgs.msg.Int32());
            Rclcs.SpinOnce(node, context, 0.1);

            Assert.That(callbackTriggered, Is.True);
        }
        public void SubscriptionCallbackMessageData()
        {
            int messageData = 12345;

            node.CreateSubscription <std_msgs.msg.Int32>("subscription_test_topic", (msg) => { messageData = msg.Data; });
            std_msgs.msg.Int32 published_msg = new std_msgs.msg.Int32();
            published_msg.Data = 42;
            publisher.Publish(published_msg);
            Rclcs.SpinOnce(node, context, 0.1);

            Assert.That(messageData, Is.EqualTo(42));
        }
Beispiel #5
0
        public void SubscriptionTriggerCallback()
        {
            bool callbackTriggered = false;

            subscriptionNode.CreateSubscription <std_msgs.msg.Float64MultiArray>("subscription_test_topic", (msg) => { callbackTriggered = true; });

            std_msgs.msg.Float64MultiArray largeMsg = new std_msgs.msg.Float64MultiArray();
            double[] data = new double[1024];
            largeMsg.data = data.OfType <double>().ToList();

            publisher.Publish(largeMsg);
            Rclcs.SpinOnce(subscriptionNode, 0.1);

            Assert.That(callbackTriggered, Is.True);
        }
    new void FixedUpdate()
    {
        Vector3 deltaPosition = (BaseRigidbody.transform.forward * LinearForwardVelocity) * Time.fixedDeltaTime;

        if (IsHolonomic)
        {
            deltaPosition += BaseRigidbody.transform.right * LinearRightVelocity * Time.fixedDeltaTime;
        }

        BaseRigidbody.MovePosition(this.BaseRigidbody.position + deltaPosition);

        Quaternion deltaRotation = Quaternion.Euler(new Vector3(0.0f, AngularVelocity * Mathf.Rad2Deg, 0.0f) *
                                                    Time.fixedDeltaTime);

        this.BaseRigidbody.MoveRotation(this.BaseRigidbody.rotation * deltaRotation);

        Rclcs.SpinOnce(node, 0.0d);
    }
        public void SubscriptionQosDefaultDepth()
        {
            int count = 0;

            node.CreateSubscription <std_msgs.msg.Int32>("subscription_test_topic",
                                                         (msg) => { count += 1; });

            std_msgs.msg.Int32 published_msg = new std_msgs.msg.Int32();
            published_msg.Data = 42;

            for (int i = 0; i < 10; i++)
            {
                publisher.Publish(published_msg);
            }

            for (int i = 0; i < 11; i++)
            {
                Rclcs.SpinOnce(node, context, 0.1);
            }

            Assert.That(count, Is.EqualTo(10));
        }
Beispiel #8
0
        public void SubscriptionQosSensorDataDepth()
        {
            int count = 0;
            QualityOfServiceProfile qosProfile = new QualityOfServiceProfile(QosProfiles.SENSOR_DATA);

            node.CreateSubscription <std_msgs.msg.Int32>("subscription_test_topic",
                                                         (msg) => { count += 1; },
                                                         qosProfile);

            std_msgs.msg.Int32 published_msg = new std_msgs.msg.Int32();
            published_msg.data = 42;

            for (int i = 0; i < 6; i++)
            {
                publisher.Publish(published_msg);
            }

            for (int i = 0; i < 11; i++)
            {
                Rclcs.SpinOnce(node, 0.1);
            }

            Assert.That(count, Is.EqualTo(5));
        }