rosserial_arduino subscriber can't hear msgs from nodes - but can hear msgs from rostopic?
JulianLelandBell opened this issue · 0 comments
I am using a Mega 2560 + Ethernet Shield 2 + rosserial_arduino to listen for messages from a PC-based ROS network (Noetic, Ubuntu 20.04). I am using a custom message definition called "ItemDispense.msg" with the following definition:
uint8 shelfID
string itemsToTrigger
Here's a defeatured-and-anonymized-but-working example of how this code is used:
#include <Arduino.h>
#include <Ethernet.h>
#include <supportingFunctions.h>
// To use the TCP version of rosserial_arduino
#define ROSSERIAL_ARDUINO_TCP
#include <ros.h>
#include <project_name/ItemDispense.h>
// Define controller ID
// TODO: convert this to use MAC address instead
const int deviceID = 0;
const String deviceID_string = String(deviceID);
// Set the shield settings
byte mac[] = { 0xA8, 0x61, 0x0A, 0xAE, 0x95, 0x44 };
IPAddress ip(192, 168, 0, 179);
// Set the rosserial socket server IP address
IPAddress server(192,168,0,11);
// Set the rosserial socket server port
const uint16_t serverPort = 11411;
ros::NodeHandle nh;
project_name::ItemDispense itemdisp_msg;
ros::Publisher pub_ID("itemDispense", &itemdisp_msg);
int localItemsToTrigger[20] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
void cb_ItemDispense( const project_name::ItemDispense& itemdisp_msg){
// Take received string and put into itemsToTrigger as ints
Serial.print("Received dispense command for Shelf ");
Serial.println(itemdisp_msg.shelfID);
Serial.print('Sending dispense command: ');
for (int i = 0; i<20; i++){
localItemsToTrigger[i] = itemdisp_msg.itemsToTrigger[i]-'0';
Serial.print(localItemsToTrigger[i]);
}
Serial.println();
dispense(localItemsToTrigger);
}
ros::Subscriber<project_name::ItemDispense> sub_ID("itemDispense", &cb_ItemDispense);
void setup() {
Wire.begin(); // Initialize I2C communications as Master
Serial.begin(115200); // Setup Serial Monitor at 9600 baud
// Connect the Ethernet
Ethernet.begin(mac, ip);
// Let some time for the Ethernet Shield to be initialized
delay(1000);
Serial.println("");
Serial.println("Ethernet connected");
Serial.println("IP address: ");
Serial.println(Ethernet.localIP());
// Set the connection to rosserial socket server
nh.getHardware()->setConnection(server, serverPort);
nh.initNode();
// Start to be polite
nh.advertise(pub_ID);
nh.subscribe(sub_ID);
}
void loop() {
nh.spinOnce();
delay(10);
}
Here's what's weird. When I try to send an ItemDispense message to this Arduino node from another ROS node on my PC (example code below), it does nothing. rqt_graph sees the communication; other nodes on the PC can hear the message; rostopic echo repeats it; but the Arduino just sits there. However, when I send the exact same message manually with rostopic:
rostopic pub itemDispense project_name/ItemDispense '{shelfID: 0, itemsToTrigger: "10111110000000000001"}'
it works perfectly!
I should also note that I've implemented other communications between PC-based ROS nodes and my Arduino successfully - including having the Arduino publish and subscribe to topics with very similar message formats - e.g. a uint8 and a string.
I'm very confused about what's going on here. This may actually be an issue with how my other ROS node is behaving - e.g. I'm formatting the message incorrectly - but since every other node/sink catches the message correctly, I'm focusing first on rosserial. Any thoughts or suggestions appreciated!
Example code for PC node that's trying to send messages:
#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
from project_name.msg import ItemDispense
target_shelf_id = 0
pub_ID = rospy.Publisher('itemDispense', ItemDispense, queue_size=1)
def cb_ItemDispense(data):
rospy.loginfo(rospy.get_caller_id() + ": Heard an item dispense call!")
def sCommander():
rospy.init_node('sCommander', anonymous=True)
rate = rospy.Rate(10)
sus_ID = rospy.Subscriber("itemDispense", ItemDispense, cb_ItemDispense)
itemDispenseCommand = IackageDispense()
itemDispenseCommand.shelfID = target_shelf_id
itemDispenseCommand.itemsToTrigger = "00011110000000000000"
pub_ID.publish(itemDispenseCommand)
rospy.loginfo(rospy.get_caller_id() + ": Sent dispense command %s to shelf ID %d" % (itemDispenseCommand.itemsToTrigger, itemDispenseCommand.shelfID))
# spin() simply keeps python from exiting until this node is stopped
while not rospy.is_shutdown():
rate.sleep()
if __name__ == '__main__':
sCommander()