| Note: This tutorial assumes you have completed previous tutorial siemens_cp1616/Tutorials/CP1616 in IO Controller mode - Configuration. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
CP1616 in IO Controller mode - User node
Description: This tutorial teaches you how to write a simple ROS node for interfacing PROFINETKeywords: SIEMENS, PROFINET, CP1616, IO Controller, User node
Tutorial Level: INTERMEDIATE
Next Tutorial: siemens_cp1616/Tutorials/CP1616 in IO Controller mode - Runtime
This tutorial teaches you how to publish and subscribe data to topics generated by PROFINET IO Controller wrapper node. Following code is intended for use with hardware configuration described in previous tutorial. Complete IO Controller tutorial package is available in github siemens_tutorials repository.
The code
1 #include <ros/ros.h>
2 #include <std_msgs/MultiArrayDimension.h>
3 #include <std_msgs/UInt8MultiArray.h>
4
5 //declare callback functions
6 void subByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
7 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
8 void subByteArrayCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
9
10 int main(int argc, char *argv[])
11 {
12 ros::init(argc, argv, "cp1616_io_controller_tutorial_node");
13 ros::NodeHandle nh;
14
15 //Subscribe to topics defined in yaml config file
16 ros::Subscriber sub_byte = nh.subscribe("/cp_input_byte_topic",
17 1, &subByteCallback);
18 ros::Subscriber sub_real = nh.subscribe("/cp_input_real_topic",
19 1, &subRealCallback);
20 ros::Subscriber sub_byte_array = nh.subscribe("/cp_input_byte_array_topic",
21 1, &subByteArrayCallback);
22
23 //Create publihsers for topics defined in yaml config file
24 ros::Publisher pub_byte = nh.advertise<std_msgs::UInt8MultiArray>
25 ("/cp_output_byte_topic", 1);
26 ros::Publisher pub_real = nh.advertise<std_msgs::UInt8MultiArray>
27 ("/cp_output_real_topic", 1);
28 ros::Publisher pub_byte_array = nh.advertise<std_msgs::UInt8MultiArray>
29 ("/cp_output_byte_array_topic", 1);
30
31 //Define and initialize output variables
32 unsigned char output_byte = 0;
33 float output_real = 0;
34 unsigned char output_byte_array[] = {0, 0, 0, 0, 0, 0, 0, 0};
35
36 //std_msgs::MultiArray variables
37 std_msgs::MultiArrayDimension msg_dim;
38 std_msgs::UInt8MultiArray msg;
39
40 while(ros::ok())
41 {
42 //-------------------------------------------
43 //Increment and publish CP Output byte
44 //-------------------------------------------
45 msg_dim.label = "CP_output_byte";
46 msg_dim.size = 1;
47 msg.layout.dim.clear();
48 msg.layout.dim.push_back(msg_dim);
49
50 output_byte++;
51 msg.data.clear();
52 msg.data.push_back(output_byte);
53 pub_byte.publish(msg);
54
55 //-------------------------------------------
56 //Increment and publish CP Output real
57 //-------------------------------------------
58 output_real += 0.1;
59 msg_dim.label = "CP_output_real";
60 msg_dim.size = 4;
61 msg.layout.dim.clear();
62 msg.layout.dim.push_back(msg_dim);
63
64 msg.data.clear();
65
66 //Decompose float to 4 bytes
67 unsigned char *p_byte;
68 unsigned int output_array[sizeof(float)];
69
70 p_byte = (unsigned char*)(&output_real);
71 for(size_t i = 0; i < sizeof(float); i++)
72 {
73 output_array[i] = (static_cast<unsigned int>(p_byte[i]));
74 msg.data.push_back(output_array[3-i]);
75 }
76 pub_real.publish(msg);
77
78 //-------------------------------------------
79 //Increment and publish CP Output byte array
80 //-------------------------------------------
81 for(size_t i = 0; i < 8; i++)
82 output_byte_array[i] += 1;
83
84 msg_dim.label = "CP_output_byte_array";
85 msg_dim.size = 8;
86 msg.layout.dim.clear();
87 msg.layout.dim.push_back(msg_dim);
88
89 msg.data.clear();
90 for(size_t i = 0; i < 8; i++)
91 msg.data.push_back(output_byte_array[i]);
92
93 pub_byte_array.publish(msg);
94
95 //-------------------------------------------
96 //Sleep and SpinOnce
97 //-------------------------------------------
98 ros::Duration(0.5).sleep();
99 ros::spinOnce();
100 }
101
102 return (EXIT_SUCCESS);
103 }
104
105 // cp_input_byte_topic message recieved
106 void subByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
107 {
108 unsigned int recieved_byte = msg->data[0];
109 }
110
111 // cp_input_real_topic message recieved
112 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
113 {
114 unsigned int raw[4];
115
116 raw[0] = msg->data[0];
117 raw[1] = msg->data[1];
118 raw[2] = msg->data[2];
119 raw[3] = msg->data[3];
120
121 //RAW bytes to float conversion
122 int sign = (raw[0] >> 7) ? -1 : 1;
123 int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;
124 uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];
125
126 float fraction = 0.5f;
127 for (uint8_t ii = 0; ii < 24; ++ii)
128 fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));
129
130 float significand = sign * fraction;
131 float recieved_real = ldexpf(significand, exponent);
132 }
133
134 // cp_input_byte_array_topic message recieved
135 void subByteArrayCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
136 {
137 unsigned int recieved_byte_array[8];
138
139 for(unsigned int i = 0; i < 8; i++)
140 recieved_byte_array[i] = msg->data[i];
141 }
The code explained
In addition to ros.h we also need to include std_msgs::UInt8MultiArray headers.
Declare callback functions for subscribers.
Subscribe to input topics defined in yaml config file.
Create publishers for output topics defined in yaml config file.
24 ros::Publisher pub_byte = nh.advertise<std_msgs::UInt8MultiArray>
25 ("/cp_output_byte_topic", 1);
26 ros::Publisher pub_real = nh.advertise<std_msgs::UInt8MultiArray>
27 ("/cp_output_real_topic", 1);
28 ros::Publisher pub_byte_array = nh.advertise<std_msgs::UInt8MultiArray>
29 ("/cp_output_byte_array_topic", 1);
Create std_msgs::MultiArray variables
To send a message in MultiArray format to the wrapper, you need to fill the "label" and "size" in "msg.layout.dim" structure in accordance with yaml config file first.
Clear old data, update and publish to the output topic
In case when a float number (REAL in STEP7 format) needs to be sent, divide float to 4 bytes by following snippet of code
66 //Decompose float to 4 bytes
67 unsigned char *p_byte;
68 unsigned int output_array[sizeof(float)];
69
70 p_byte = (unsigned char*)(&output_real);
71 for(size_t i = 0; i < sizeof(float); i++)
72 {
73 output_array[i] = (static_cast<unsigned int>(p_byte[i]));
74 msg.data.push_back(output_array[3-i]);
75 }
76 pub_real.publish(msg);
Subscribing to MultiArray topic in case when only single byte is being transmitted:
When a REAL number is being transmitted, to convert 4 bytes back to float type, following code can be used:
112 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
113 {
114 unsigned int raw[4];
115
116 raw[0] = msg->data[0];
117 raw[1] = msg->data[1];
118 raw[2] = msg->data[2];
119 raw[3] = msg->data[3];
120
121 //RAW bytes to float conversion
122 int sign = (raw[0] >> 7) ? -1 : 1;
123 int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;
124 uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];
125
126 float fraction = 0.5f;
127 for (uint8_t ii = 0; ii < 24; ++ii)
128 fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));
129
130 float significand = sign * fraction;
131 float recieved_real = ldexpf(significand, exponent);
132 }
If the code is ready and built, we can move to running the wrapper and checking the results:
Next tutorial: CP1616 - In IO Controller mode - Runtime
