ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

How to send messages from a node running python to arduino?

asked 2018-01-15 09:22:23 -0500

JollyRogers gravatar image

updated 2018-01-15 09:45:01 -0500

gvdhoorn gravatar image

I upload the following code to Arduino.

#include <ros.h>
#include <std_msgs/Float64.h>
ros::NodeHandle nh;

void messageCb(const std_msgs::Float64& msg)
{
  if(msg.data > 1.0)
    digitalWrite(13, HIGH);   // blink the led
    else 
    digitalWrite(13, LOW);    // Turn off led
}

ros::Subscriber<std_msgs::Float64> sub("your_topic", &messageCb);

void setup()
{
  pinMode(13, OUTPUT);
  nh.initNode();
  nh.subscribe(sub);
}
void loop()
{

 nh.spinOnce();
 delay(1);
}

I use this code to turn on a LED connected to PIN13. The code receives a signal from the topic "your_topic", then if the signal is bigger than 1.0 it turns the LED.

From the terminal, I use the next code to publish to "your_topic": rostopic pub your_topic std_msgs/Float64 2

Though this code works, I now want to publish using python. I tried the succeeding code.

#!/usr/bin/env python
import rospy
from project_1.msg import *
from random import randint

rospy.init_node('Arduino_hear_my_whispers')
pub = rospy.Publisher('your_topic', Arduino_message, queue_size=10)
rnd = randint(0,2)
print "the number is " + str(rnd)
message=Arduino_message(x=rnd)
pub.publish(message)
rate = rospy.Rate(10)
rate.sleep()

Although the code worked, the LED did not turn on. Can someone help me?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
2

answered 2018-01-15 11:07:04 -0500

jayess gravatar image

There's a couple of things going on here:

  1. What are the contents of Arduino_message? This looks like it's a custom message but we don't know what it looks like
  2. The your_topic topic from the Arduino's perspective is expecting a Float64 message but your Python node is publishing an Arduino_message message (containing an int for the x parameter)
  3. You're not looping so you're only publishing once which means you'll have to keep re-running the node until you get 2 (which may be what you want, but IMO is tedious to test)

This should help:

  1. Post the contents of your custom Arduino_message so we can see what's going on there
  2. Match the data (message) type of the your_topic topic. Either go with your custom Arduino_message, a Float64, or an int
  3. At least test this out using a loop until you get it working.

Also, I notice that you have a print statement in there. Can you please update your question with a copy and paste of the result of it? Otherwise this answer is just a guess.

edit flag offensive delete link more

Comments

Here is the content of the Arduino_message.msg file: float64 x I also run the script in a loop and found the following error: [WARN] [1516049736.697616][std_msgs/Float64] vs. [project_1/Arduino_message] ... It is like the format std_msgs/Float64 and float64 are different.

JollyRogers gravatar image JollyRogers  ( 2018-01-15 14:56:44 -0500 )edit

@JollyRogers: please update your question with the full error and the definition

jayess gravatar image jayess  ( 2018-01-15 15:26:21 -0500 )edit
1

answered 2018-01-15 15:36:00 -0500

JollyRogers gravatar image

updated 2018-01-15 16:55:50 -0500

jayess gravatar image

I solved this problem importing FLoat 64 from std_msg.msg. I still do not know why ROS is not recognizing the float64 type from the Arduino_message file. The final code is presented below:

#!/usr/bin/env python
import rospy
from std_msgs.msg import  Float64
from random import randint
from time import sleep

rospy.init_node('Arduino_hear_my_whispers')
pub = rospy.Publisher('your_topic', Float64, queue_size=10)
while not rospy.is_shutdown():
        rnd = randint(0,2)
        print "the number is " + str(rnd)
        message=Float64(rnd)
        pub.publish(message)
        rate = rospy.Rate(100)
        sleep(3)
rate.sleep()
edit flag offensive delete link more

Comments

So, basically what I suggested? I think that you should go through the ROS tutorials for rospy again. You should use the Rate object's sleep() method instead of Python's. Also, you should review how to use custom messages.

jayess gravatar image jayess  ( 2018-01-15 17:00:15 -0500 )edit

If you still can't figure out how to use your custom message then feel free to ask a new question.

jayess gravatar image jayess  ( 2018-01-15 17:00:45 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-01-15 09:22:23 -0500

Seen: 1,177 times

Last updated: Jan 15 '18