Anomalous Behavior in ROS2 Unity Project: Issue with Receiving Topic Data

[jvmoraiscb]

I am working on a project (https://github.com/jvmoraiscb/rhcr) with ROS2 in Unity using this library.

In a virtual machine, I run a node with topics related to a controller (it publishes the position and receives the force it should apply). The node is within a container that I haven't modified for some time.

However, in the last week, a very strange error has started to occur, and I have no idea what it could be: when I run the program in Unity and in the container, the controller behaves anomalously (as if it were receiving garbage from the topic related to force). However, if I open another terminal and run "ros2 topic echo /force_vector," the controller stops moving (which is normal because I am not sending any force values in Unity), and the project works normally.

These are the codes related to the control node (I ignored falcon.cpp as it only abstracts control functions):

main.cpp:

#include <csignal>

#include "falcon_node.hpp"

// global pointers
bool* debug_mode_pt;
Falcon* falcon_pt;

void signal_handler(int sig) {
    if (sig == SIGTSTP) {
        if (*debug_mode_pt) {
            *debug_mode_pt = false;
            falcon_pt->print_info();
        } else {
            *debug_mode_pt = true;
        }
    }
}

int main(int argc, char* argv[]) {
    // initialize falcon variables
    auto falcon = Falcon();
    auto debug_mode = false;

    // initialize global pointers
    debug_mode_pt = &debug_mode;
    falcon_pt = &falcon;

    // register custom handler to ctrl-z
    signal(SIGTSTP, signal_handler);

    // initialize ros variables
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<Falcon_Node>(falcon_pt, debug_mode_pt));
    rclcpp::shutdown();

    return 0;
}

falcon_node.cpp:

#include "falcon_node.hpp"

using namespace std;
using namespace chrono_literals;

Falcon_Node::Falcon_Node(Falcon* falcon, bool* debug_mode) : Node("falcon_node"), count_(0) {
    falcon_ = falcon;
    debug_mode_ = debug_mode;
    timer_ = this->create_wall_timer(10ms, std::bind(&Falcon_Node::timer_callback, this));

    position_vector_pub = this->create_publisher<geometry_msgs::msg::Vector3>("position_vector", 10);
    right_button_pub = this->create_publisher<std_msgs::msg::Int16>("right_button", 10);
    up_button_pub = this->create_publisher<std_msgs::msg::Int16>("up_button", 10);
    center_button_pub = this->create_publisher<std_msgs::msg::Int16>("center_button", 10);
    left_button_pub = this->create_publisher<std_msgs::msg::Int16>("left_button", 10);
    force_vector_sub = this->create_subscription<geometry_msgs::msg::Vector3>("force_vector", 10, std::bind(&Falcon_Node::force_callback, this, std::placeholders::_1));
    rgb_vector_sub = this->create_subscription<geometry_msgs::msg::Vector3>("rgb_vector", 10, std::bind(&Falcon_Node::rgb_callback, this, std::placeholders::_1));

    printf("Please calibrate the controller: move it around and then press the center button.\n");

    falcon_->rgb(true, false, false);
    falcon_->calibrate();
    falcon_->rgb(false, true, false);

    falcon_->print_info();
}

void Falcon_Node::timer_callback() {
    double x, y, z;
    int button1, button2, button3, button4;

    falcon_->update();
    falcon_->get(&x, &y, &z, &button1, &button2, &button3, &button4);

    if (*debug_mode_) {
        printf("X: %+.2f | Y: %+.2f | Z: %+.2f | B1: %d | B2: %d | B3: %d | B4: %d\n", x, y, z, button1, button2, button3, button4);
    }

    auto pos = geometry_msgs::msg::Vector3();
    pos.x = (float)x;
    pos.y = (float)y;
    pos.z = (float)z;

    auto right = std_msgs::msg::Int16();
    right.data = button1;

    auto up = std_msgs::msg::Int16();
    up.data = button2;

    auto center = std_msgs::msg::Int16();
    center.data = button3;

    auto left = std_msgs::msg::Int16();
    left.data = button4;

    position_vector_pub->publish(pos);
    right_button_pub->publish(right);
    up_button_pub->publish(up);
    center_button_pub->publish(center);
    left_button_pub->publish(left);
}

void Falcon_Node::force_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
    falcon_->set(msg->x, msg->y, msg->z);
}

void Falcon_Node::rgb_callback(const geometry_msgs::msg::Vector3::SharedPtr msg) {
    bool red = ((int)msg->x == 1) ? true : false;
    bool green = ((int)msg->y == 1) ? true : false;
    bool blue = ((int)msg->z == 1) ? true : false;

    falcon_->rgb(red, green, blue);
}

and this is the code for the node in Unity:

Ros2FalconUnityNode.cs

using UnityEngine;

namespace ROS2
{
    public class Ros2FalconNode : MonoBehaviour
    {
        [SerializeField]
        private FalconEnv falconEnv;
        [SerializeField]
        private string nodeName;
        [SerializeField]
        private string positionTopicName;
        [SerializeField]
        private string rightButtonTopicName;
        [SerializeField]
        private string upButtonTopicName;
        [SerializeField]
        private string centerButtonTopicName;
        [SerializeField]
        private string leftButtonTopicName;
        [SerializeField]
        private string forceTopicName;
        [SerializeField]
        private string rgbTopicName;

        private ROS2UnityComponent ros2Unity;
        private ROS2Node ros2Node;
        private IPublisher<geometry_msgs.msg.Vector3> force_pub;
        private IPublisher<geometry_msgs.msg.Vector3> rgb_pub;

        private const int RIGHT = 1;
        private const int UP = 2;
        private const int CENTER = 3;
        private const int LEFT = 4;
        private int lastStatus_right = -1;
        private int lastStatus_up = -1;
        private int lastStatus_center = -1;
        private int lastStatus_left = -1;

        void Start()
        {
            ros2Unity = GetComponent<ROS2UnityComponent>();
            if (ros2Node == null)
                ros2Node = ros2Unity.CreateNode(nodeName);
            if (ros2Unity.Ok())
            {
                ros2Node.CreateSubscription<geometry_msgs.msg.Vector3>(positionTopicName, msg => PositionHandler(msg.X, msg.Y, msg.Z));
                ros2Node.CreateSubscription<std_msgs.msg.Int16>(rightButtonTopicName, msg => ButtonHandler(RIGHT, msg.Data));
                ros2Node.CreateSubscription<std_msgs.msg.Int16>(upButtonTopicName, msg => ButtonHandler(UP, msg.Data));
                ros2Node.CreateSubscription<std_msgs.msg.Int16>(centerButtonTopicName, msg => ButtonHandler(CENTER, msg.Data));
                ros2Node.CreateSubscription<std_msgs.msg.Int16>(leftButtonTopicName, msg => ButtonHandler(LEFT, msg.Data));

                force_pub = ros2Node.CreatePublisher<geometry_msgs.msg.Vector3>(forceTopicName);
                rgb_pub = ros2Node.CreatePublisher<geometry_msgs.msg.Vector3>(rgbTopicName);
            }
        }

        void Update()
        {
            if (ros2Unity.Ok())
            {
                ForceHandler();
                RgbHandler();
            }

        }

        void ForceHandler()
        {
            geometry_msgs.msg.Vector3 msg = new geometry_msgs.msg.Vector3
            {
                X = falconEnv.force.x,
                Y = falconEnv.force.y,
                Z = falconEnv.force.z
            };

            force_pub.Publish(msg);
        }

        void RgbHandler()
        {
            geometry_msgs.msg.Vector3 msg = new geometry_msgs.msg.Vector3
            {
                X = falconEnv.rgb.x,
                Y = falconEnv.rgb.y,
                Z = falconEnv.rgb.z
            };

            rgb_pub.Publish(msg);
        }

        void PositionHandler(double x, double y, double z)
        {
            falconEnv.position.x = (float)x;
            falconEnv.position.y = (float)y;
            falconEnv.position.z = (float)z;
        }

        void ButtonHandler(int button, int value)
        {
            if (button == RIGHT && value != lastStatus_right) {
                if (lastStatus_right != -1)
                    falconEnv.RightButtonHandler();
                lastStatus_right = value;
            }
            if (button == UP && value != lastStatus_up) {
                if (lastStatus_up != -1)
                    falconEnv.UpButtonHandler();
                lastStatus_up = value;
            }
            if (button == CENTER && value != lastStatus_center) {
                if (lastStatus_center != -1)
                    falconEnv.CenterButtonHandler();
                lastStatus_center = value;
            }
            if (button == LEFT && value != lastStatus_left) {
                if (lastStatus_left != -1)
                    falconEnv.LeftButtonHandler();
                lastStatus_left = value;
            }
        }
    }

}

I tried reverting the project to previous versions where everything was working, attempted to recompile the project, but the error persists. I also tried using ros2bag to check the state, but when I ran the program, it subscribed to the topic, and the project started working correctly again.

Does anyone have any idea what I could do to solve this problem?

[jvmoraiscb]

I switched from 'standalone' mode to 'overlay,' and the project started working again.