You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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:
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()
The text was updated successfully, but these errors were encountered:
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:
Here's a defeatured-and-anonymized-but-working example of how this code is used:
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:
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:
The text was updated successfully, but these errors were encountered: