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 CreateNodeWithInvalidName()
{
string nodeName = "create_node_test_invaild_name?";
Assert.That(() => { Rclcs.CreateNode(nodeName, context).DestroyNode(); },
Throws.TypeOf <InvalidNodeNameException>());
}
public void SetUp()
{
context = new Context();
Rclcs.Init(context);
node = new Node("subscription_test_node", context);
publisher = node.CreatePublisher <std_msgs.msg.Int32>("subscription_test_topic");
}
public void InitShutdown()
{
Context context = new Context();
Rclcs.Init(context);
Rclcs.Shutdown(context);
}
public void SetUp()
{
context = new Context();
Rclcs.Init(context);
node = new Node("test_node", context);
Subscription <std_msgs.msg.Int64> subscription = node.CreateSubscription <std_msgs.msg.Int64>("/test_topic", (msg) => { });
}
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 TearDown()
{
pub_node.Dispose();
sub_node.Dispose();
Rclcs.Shutdown(pub_context);
Rclcs.Shutdown(sub_context);
}
public static void Main(string[] args)
{
Context ctx = new Context();
Rclcs.Init(ctx);
INode node = Rclcs.CreateNode("talker", ctx);
IPublisher <std_msgs.msg.String> chatter_pub = node.CreatePublisher <std_msgs.msg.String>("chatter");
std_msgs.msg.String msg = new std_msgs.msg.String();
int i = 1;
while (Rclcs.Ok(ctx))
{
// adamdbrw - if no at least small sleep is made before
// the first published message, it doesn't reach subscribers
// Needs investigation for how to put a valid check here
// TODO (adam) replace with a proper check
Thread.Sleep(500);
msg.Data = "Hello World: " + i;
i++;
Console.WriteLine("Publishing: \"" + msg.Data + "\"");
chatter_pub.Publish(msg);
Thread.Sleep(500);
}
Rclcs.Shutdown(ctx);
}
public void CreateNodeWithInvalidRelativeNamespace()
{
string nodeName = "create_node_test_invalid_namespace";
string nodeNamespace = "invalid_namespace?";
Assert.That(() => { Rclcs.CreateNode(nodeName, context, nodeNamespace: nodeNamespace).DestroyNode(); },
Throws.TypeOf <InvalidNamespaceException>());
}
public void DoubleShutdown()
{
Context context = new Context();
Rclcs.Init(context);
Rclcs.Shutdown(context);
Assert.That(() => { Rclcs.Shutdown(context); }, Throws.TypeOf <RuntimeError>());
}
public void CreateNodeWithRelativeNamespace()
{
string nodeName = "create_node_test";
string nodeNamespace = "ns";
Node node = Rclcs.CreateNode(nodeName, context, nodeNamespace: nodeNamespace);
Assert.That(node.Namespace, Is.EqualTo("/ns"));
node.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 SetUp()
{
pub_context = new Context();
sub_context = new Context();
Rclcs.Init(pub_context);
Rclcs.Init(sub_context);
pub_node = new Node("pub_node", pub_context);
sub_node = new Node("sub_node", sub_context);
}
public void InitShutdownSequence()
{
// local
Context context = new Context();
Rclcs.Init(context);
Rclcs.Shutdown(context);
context = new Context();
Rclcs.Init(context);
Rclcs.Shutdown(context);
}
public void SetUp()
{
publisherContext = new Context();
subscriptionContext = new Context();
Rclcs.Init(publisherContext);
Rclcs.Init(subscriptionContext);
subscriptionNode = new Node("subscription_test_node", publisherContext);
publisherNode = new Node("publisher_test_node", publisherContext);
publisher = publisherNode.CreatePublisher <std_msgs.msg.Float64MultiArray>("subscription_test_topic");
}
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 static void Main(string[] args)
{
Context ctx = new Context();
Rclcs.Init(ctx);
INode node = Rclcs.CreateNode("listener", ctx);
ISubscription <std_msgs.msg.String> chatter_sub = node.CreateSubscription <std_msgs.msg.String>(
"chatter", msg => Console.WriteLine("I heard: [" + msg.Data + "]"));
Rclcs.Spin(node, ctx);
Rclcs.Shutdown(ctx);
}
public void Init()
{
Context context = new Context();
Rclcs.Init(context);
try
{
Rclcs.Shutdown(context);
}
catch (RuntimeError)
{
}
}
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));
}
public void TearDown()
{
node.Dispose();
Rclcs.Shutdown(context);
}
public void SetUp()
{
context = new Context();
Rclcs.Init(context);
node = Rclcs.CreateNode(TEST_NODE, nodeNamespace: TEST_NAMESPACE, context: context);
}
public void CreateNodeWithoutInit()
{
Context context = new Context();
Assert.That(() => { Rclcs.CreateNode("foo", context); }, Throws.TypeOf <NotInitializedException>());
}
public void TearDown()
{
publisher.Dispose();
subscriptionNode.Dispose();
Rclcs.Shutdown(publisherContext);
}
public void CreateNode()
{
string nodeName = "create_node_test";
Rclcs.CreateNode(nodeName, context).Dispose();
}
public void SetUp()
{
context = new Context();
Rclcs.Init(context);
}
public void TearDown()
{
Rclcs.Shutdown(context);
}
