Robotics StackExchange | Archived questions

Rosserial arduino publish topic in different void function

Hello Dear Community. I am trying to publish data from the Arduino but it seems it's not working well because I can't see the message publishing. I guess the arduino publish fast and when I run rosrun rosserial_python serial_node.py /dev/ttyACM0 takes time to connect.

I need to publish a topic from another void function different than void loop.

Here is my code in Arduino

#define USE_USBCON
#include <ros.h>
#include <std_msgs/UInt8.h>
#define STATUS_PROCESSING 0x31
uint8_t CURRENT_STATUS;
ros::NodeHandle  nh;
std_msgs::UInt8 str_msg;

ros::Publisher chatter("chatter", &str_msg);
void setup(){
   nh.initNode();
   nh.advertise(chatter);
   SERIAL_INIT(9600);
   Serial1.println("INIT\tOK");
}
void loop(){
   nh.spinOnce();
   delay(3000);
}
void SERIAL_INIT(int t){
  Serial1.begin(t);  
  CURRENT_STATUS = STATUS_PROCESSING;
  str_msg.data = CURRENT_STATUS;
  delay(3000);
  chatter.publish( &str_msg );
  nh.spinOnce();
  delay(1000);
}

and this is my python subscriber:

import rospy
from std_msgs.msg import UInt8
from time import sleep

def callback(data):
    rospy.loginfo("I heard: %s", data.data)


def listener():

   rospy.Subscriber("chatter", UInt8, callback)

    rospy.spin()

if __name__ == '__main__':
    rospy.init_node('listener', anonymous=True)
    listener()

Running the code, nothing happens... I need to see for one time that SERIAL INIT is done. I hope someone helps me. It seems that is publishing but when I run rosrun rosserialpython serialnode.py /dev/ttyACM0 it takes time to run son when is running the Arduino already published... So How can I fix it? Thank you so much.

Asked by subarashi on 2020-06-28 14:59:02 UTC

Comments

Not clear when you say nothing happens are you referring to not seeing a message in ros, or not seeing anything from the Serial1 port on the mega, or both.

I would tend to turn on the built - in led to provide an indication of whether the function has completed, rather than adding the complication of the additional serial port.

I don't have a mega so can't try your code as is, but when I removed the Serial1 stuff and tried your code on an Uno, I got messages appearing in rostopic echo /chatter if I called SERIAL_INIT from loop(), but not from within setup()

Asked by nickw on 2020-06-30 01:09:27 UTC

Thanks for answering me. I've solved that issue moving the publisher to the void loop... but when it is in other void functions, It seems is not publishing... Do you know a good way to publish a message from other void functions?

Asked by subarashi on 2020-06-30 02:04:33 UTC

Is this what you mean by publish in another function, this works as expected on an uno, rostopic echo /chatter

#include <ros.h>
#include <std_msgs/UInt16.h>

ros::NodeHandle  nh;
std_msgs::UInt16 theMsg;

ros::Publisher chatter("chatter", &theMsg);

void setup(){
   nh.initNode();
   nh.advertise(chatter);
}

void loop(){  
   theFunction();   
   nh.spinOnce();
   delay(100);
}

void theFunction(){  
  theMsg.data = analogRead(A0);
  chatter.publish( &theMsg );
}

Asked by nickw on 2020-06-30 03:54:39 UTC

Thank you just a few minutes I've tried and works... Thank you. Is there anyway to test the connection between Ros and Arduino? For instance test connection between Ros in raspberry with Ros Arduino?

Asked by subarashi on 2020-06-30 05:17:06 UTC

there are many things to try to troubleshoot things, if you have a specific problem probably best to ask a new question to keep stuff organised and easier to search, as this is a different issue to the original question

Asked by nickw on 2020-06-30 06:06:31 UTC

Answers

in your code arduino code you have created object ros::NodeHandle NH;

and using as nh.initNode();

please change those to lowercase

and please remove Serial.print statements Rosserial uses the arduino's serial port for communication. By writing to the same port using Serial.print you're breaking the protocol. Hope this helps.

Asked by Jayanth on 2020-06-29 06:33:59 UTC

Comments

Thank you for your answer. Was my mistake... You mean to change this ros::NodeHandle nh; to this ros::nodehandle nh; ? I am trying to publish a topic that is stored in other void function...

Asked by subarashi on 2020-06-29 06:48:19 UTC

no just use like "ros::NodeHandle nh;"

Asked by Jayanth on 2020-06-29 06:51:32 UTC

It was my mistake writing here... But as I told you, my problem is publishing in a different void function

Asked by subarashi on 2020-06-29 06:54:04 UTC