ros-drivers / rosserial

A ROS client library for small, embedded devices, such as Arduino. See: http://wiki.ros.org/rosserial
508 stars 527 forks source link

rosserial_arduino subscriber can't hear msgs from nodes - but can hear msgs from rostopic? #612

Open JulianLelandBell opened 1 year ago

JulianLelandBell commented 1 year ago

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()