Difference between revisions of "Writing ROS code"

From Bike Wiki
Jump to navigation Jump to search
(correct typo in arduino code)
(link to float32)
Line 17: Line 17:
 
#include <std_msgs/Float32.h>
 
#include <std_msgs/Float32.h>
 
</syntaxhighlight>
 
</syntaxhighlight>
|Imports the main ROS "client library" (code needed to interact with ROS) and the Float32 message type.
+
|Imports the main ROS "client library" (code needed to interact with ROS) and the [http://docs.ros.org/api/std_msgs/html/msg/Float32.html Float32 message type].
 
|-
 
|-
 
|<syntaxhighlight lang="python">
 
|<syntaxhighlight lang="python">

Revision as of 23:27, 28 February 2020

This page covers writing ROS nodes, which are programs that connect (often with TCP) to a central server program, called the ROS core. After connecting with the core, ROS nodes can communicate with each other. Three ways ROS nodes communicate are topics, services, and parameters. We mostly use topics. This page has Python and C++ (Arduino) examples for communicating between two nodes using topics.

Topics are like mailing lists. Each topic has a name and a data type. Names usually start with a forward slash, such as /bike_state. Data types are defined by ROS (or you). Here's a list of the standard ones. We mostly use Float32 and Float32MultiArray. Nodes publish to and subscribe from topics. Multiple nodes can publish to and subscribe from the same topic. There are also command-line and graphical tools for publishing to a topic and showing messages published to a topic.

Writing a node

Python (Pi) Arduino/C++ (Due) Description
import rospy
from std_msgs.msg import Float32
#include <ros.h>
#include <std_msgs/Float32.h>
Imports the main ROS "client library" (code needed to interact with ROS) and the Float32 message type.
rospy.init_node("asdf")

The node name is "asdf". Also takes an optional argument anonymous=True, which lets you simultaneously run multiple copies of the same node

typedef ros::NodeHandle_<ArduinoHardware, 1, 3, 500, 500> RosNodeHandle;
RosNodeHandle nh;

// in a function:
nh.initNode();

Note: ros::init may also be required, but we don't call it on the Arduino
Note 2: The Arduino node is given a name on the Pi, so we don't pass in a name here.

Initializes a node
pub = rospy.Publisher("hjkl", Float32, queue_size=10)
std_msgs::Float32 hjkl_data;
ros::Publisher hjkl_pub("hjkl", &hjkl_data);

// In a function:
nh.advertise(hjkl_pub);
Sets up a publisher who will publish to the topic hjkl.
pub.publish(42)
hjkl_data.data = 42;
hjkl_pub.publish(&hjkl_data);
Publishes the number 42.
def foo_callback(data):
    print(data.data)
rospy.Subscriber("foo", Float32, foo_callback)
void foo_callback(const std_msgs::Float32& data) {
    Serial.println(data.data);
}
ros::Subscriber<std_msgs::Float32> foo_sub("foo", &foo_callback);
Subscribes to the topic foo, which publishes Float32 values.

Launch files