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);
}
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));
}
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));
}
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));
}
