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):
#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?
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:
falcon_node.cpp:
and this is the code for the node in Unity:
Ros2FalconUnityNode.cs
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?