I have a node running on pc which published [std_msgs/String] in string It published M, R, S, I have the following Arduino code.
#include <ros.h>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h>
ros::NodeHandle nh;
void messageCb( const std_msgs::String & toggle_msg)
{
nh.loginfo("recived new message ");
nh.loginfo(toggle_msg.data);
if(toggle_msg.data == "M")
{
nh.loginfo("Recived M if-statment ");
}
else if(toggle_msg.data == "R")
{
nh.loginfo("Recived R if-statment ");
}
else if(toggle_msg.data == "S")
{
nh.loginfo("Recived S if-statment ");
}
}
ros::Subscriber<std_msgs::String> sub("talker_vision", &messageCb );
void setup()
{
pinMode(13, OUTPUT);
nh.initNode();
nh.subscribe(sub);
}
void loop()
{
nh.spinOnce();
delay(1);
}
I received M , R , S from nh.loginfo(toggle_msg.data); but when I applied If condition
if(toggle_msg.data == "M")
{
nh.loginfo("Recived M if-statment ");
}
there is no output.
The problem you are facing is that you are comparing two strings here which are defined as char*, not the Arduino classic String type is used. You can see the type of the string inside the message which was generated for example here.
Comparing
const char* str1 = "abc";
const char* str2 = "abc";
bool equal = str1 == str2;
will be always false since you are comparing memory positions and not the string itself.
So you have two options:
Create two Arduino string objects and compare them
String arduinoString1 = String(str1);
String arduinoString2 = String(str2);
bool equal = arduinoString1 == arduinoString2;
Or even better is to use the standard string comparision strcmp (documentation) function as described here for example:
bool equal = strcmp(str1, str2) == 0;
Related
I'm trying to use a teensy 4.1 as an interface between an encoder and ROS thanks to micro-ros (arduino version).
I would like to publish position of a wheel to the /jointState topic with the teensy but there is no example on the micro-ros arduino Github repo.
I've tried to inspect the sensormsgs/msg/jointState message struct but everything is a bit fuzzy and I don't understand how to make it works. I can't understand what is rosidl_runtime_c__double__Sequence type.
I've tried several things but I always get an error about operand types
no match for 'operator=' (operand types are 'rosidl_runtime_c__String' and 'const char [18]')
msg.name.data[0] = "drivewhl_1g_joint";
Here is my arduino code
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <sensor_msgs/msg/joint_state.h>
rcl_publisher_t publisher;
sensor_msgs__msg__JointState msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
#define LED_PIN 13
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
void error_loop(){
while(1){
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
delay(100);
}
}
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
//
//Do not work
//msg.name=["drivewhl_1g_joint","drivewhl_1d_joint","drivewhl_2g_joint","drivewhl_2d_joint"];
//msg.position=["1.3","0.2", "0","0"];
msg.name.size = 1;
msg.name.data[0] = "drivewhl_1g_joint";
msg.position.size = 1;
msg.position.data[0] = 1.85;
}
}
void setup() {
set_microros_transports();
pinMode(LED_PIN, OUTPUT);
digitalWrite(LED_PIN, HIGH);
delay(2000);
allocator = rcl_get_default_allocator();
//create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support));
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
"JointState"));
// create timer,
const unsigned int timer_timeout = 1000;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
}
void loop() {
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
I'm a beginner with Ros and C, it may be a very dumb question but I don't know how to solve it. Thanks for your help !
rosidl_runtime_c__String__Sequence is a structure used to old string data that is to be transmitted. Specifically it is a sequence of rosidl_runtime_c__String data. You're running into an error because rosidl_runtime_c__String is also a struct itself with no custom operators defined. Thus, your assignment fails since the types are not directly convertible. What you need to do instead is use the rosidl_runtime_c__String.data field. You can see slightly more info here
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
//msg.name=["drivewhl_1g_joint","drivewhl_1d_joint","drivewhl_2g_joint","drivewhl_2d_joint"];
//msg.position=["1.3","0.2", "0","0"];
msg.name.size = 1;
msg.name.data[0].data = "drivewhl_1g_joint";
msg.name.data[0].size = 17; //Size in bytes excluding null terminator
msg.position.size = 1;
msg.position.data[0] = 1.85;
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
}
}
I also spent quite some time trying to get publishing JointState message from my esp32 running microros, and also couldn't find working example. Finally, i was successful, maybe it will help someone.
In simple words:
.capacity contains max number of elements
.size contains actual number of elements (strlen in case of string)
.data should be allocated as using malloc as .capacity * sizeof()
each string within sequence should be allocated separately
This is my code that allocates memory for 12 joints, named j0-j11. Good luck!
...
// Declarations
rcl_publisher_t pub_joint;
sensor_msgs__msg__JointState joint_state_msg;
...
// Create publisher
RCCHECK(rclc_publisher_init_default(&pub_joint, &node,
ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
"/hexapod/joint_state"));
//Allocate memory
joint_state_msg.name.capacity = 12;
joint_state_msg.name.size = 12;
joint_state_msg.name.data = (std_msgs__msg__String*) malloc(joint_state_msg.name.capacity*sizeof(std_msgs__msg__String));
for(int i=0;i<12;i++) {
joint_state_msg.name.data[i].data = malloc(5);
joint_state_msg.name.data[i].capacity = 5;
sprintf(joint_state_msg.name.data[i].data,"j%d",i);
joint_state_msg.name.data[i].size = strlen(joint_state_msg.name.data[i].data);
}
joint_state_msg.position.size=12;
joint_state_msg.position.capacity=12;
joint_state_msg.position.data = malloc(joint_state_msg.position.capacity*sizeof(double));
joint_state_msg.velocity.size=12;
joint_state_msg.velocity.capacity=12;
joint_state_msg.velocity.data = malloc(joint_state_msg.velocity.capacity*sizeof(double));
joint_state_msg.effort.size=12;
joint_state_msg.effort.capacity=12;
joint_state_msg.effort.data = malloc(joint_state_msg.effort.capacity*sizeof(double));
for(int i=0;i<12;i++) {
joint_state_msg.position.data[i]=0.0;
joint_state_msg.velocity.data[i]=0.0;
joint_state_msg.effort.data[i]=0.0;
}
....
//Publish
RCSOFTCHECK(rcl_publish(&pub_joint, &joint_state_msg, NULL));
Hi I tried to write my own version of memmove and I find the following code resulting in a segmentation fault. It would be great if someone could help me figure out why this behavior would occur!
However, when I use something like:
char source[20] = "Hello, this is Piranava", the code works fine!
void *memmoveLocal(void *dest, const void *src, unsigned int n)
{
char *destL = dest;
const char *srcL = src;
int i = 0;
if(dest == NULL || src == NULL)
{
return NULL;
}
else
{
// if dest comes before source, even if there's an overlap, we should move forward
// because if there's an overlap (when dest < src) and we move backward, we'd overwrite the overlapping bytes in src
if(destL < srcL)
{
printf("Forward\n");
while(i < n)
{
destL[i] = srcL[i];
i++;
}
}
else // in all other cases (even if there's overlap or no overlap, we can move backward)
{
printf("Backward\n");
i = n - 1;
while(i >= 0)
{
destL[i] = srcL[i];
i--;
}
}
}
return dest;
}
void main()
{
char *source = "Hello, this is ABC";
char *destination = malloc(strlen(source)+1);
memmoveLocal(source+5, source, 5);
printf("Source: %s \nDestination: %s, size: %d\n", source, destination, strlen(destination));
}
However, if I replace
char *source = "Hello, this is ABC";
with
char source[20] = "Hello, this is ABC";
, it works fine!
memmoveLocal(source+5, source, 5);
You are trying to overwrite a string literal, which is not writable.
Did you intend to memmoveLocal(destination, source+5, 5) instead?
char source[20] = "Hello, this is ABC";
That turns source from a string literal into a char[] array initialized with a string literal. The array is writable, so your program no longer crashes.
I'm struggling with an issue where an ESP32 is running as a AP with AsyncTCP connecting multiple ESP32 clients. The AP receives some JSON data and replies with some JSON data. Without the handleData() function, the code runs 100% fine with no issues. Heap is static when no clients connect and issues only occur when clients start connecting.
Can anyone see anything with my code that could be causing heap corruption or other memory weirdness?
static void handleData(void* arg, AsyncClient* client, void *data, size_t len) {
int i = 0, j = 0;
char clientData[CLIENT_DATA_MAX];
char packetData[len];
char *packetBuf;
packetBuf = (char *)data;
clientData[0] = '\0';
for (i=0;i <= len;i++) {
packetData[j] = packetBuf[i]; //packetBuf[i];
if ((packetData[j] == '\n') || (i == len)) {
packetData[j] = '\0';
if ((j > 0) && (packetData[0] != '\n') && (packetData[0] != '\r')) {
// See sensorData() below...
parseData.function(packetData, clientData);
if (clientData != NULL) {
// TCP reply to client
if (client->space() > 32 && client->canSend()) {
client->write(clientData);
}
}
}
j = 0;
} else
j++;
}
}
void sensorData(void *data, void *retData) {
StaticJsonDocument<CLIENT_DATA_MAX> fields;
StaticJsonDocument<CLIENT_DATA_MAX> output;
char sensor[15] = "\0";
char MAC[18] = "\0";
char value[20] = "\0";
bool sendOK = false;
memcpy((char *)retData, "\0", 1);
DeserializationError error = deserializeJson(fields, (char *)data, CLIENT_DATA_MAX);
if (error) {
DEBUG_PRINTLN(F("deserializeJson() failed"));
return;
}
if (fields["type"])
strcpy(sensor, fields["type"]);
switch (sensor[0]) {
case 'C':
if (fields["value"])
strcpy(value, fields["value"]);
sendOK = true;
break;
case 'T': //DEBUG_PRINT(F("Temp "));
setExtTempSensor(fields["value"]);
sendOK = true;
break;
case 'N':
output["IT"] = intTempC; //Internal temp
output["B1"] = battLevels[0];
serializeJson(output, (char *)retData, CLIENT_DATA_MAX-1);
break;
}
if (sendOK) {
output["Resp"] = "Ok";
serializeJson(output, (char *)retData, CLIENT_DATA_MAX-1);
}
strcat((char *)retData, "\n");
}
static void handleNewClient(void* arg, AsyncClient* client) {
client->setRxTimeout(1000);
client->setAckTimeout(500);
client->onData(&handleData, NULL);
client->onError(&handleError, NULL);
client->onDisconnect(&handleDisconnect, NULL);
client->onTimeout(&handleTimeOut, NULL);
}
void startServer() {
server = new AsyncServer(WIFI_SERVER_PORT);
server->onClient(&handleNewClient, &server)
}
Using AsyncTCP on the ESP32 was having multiple issues. Heap issues, socket issues, assert issues, ACK timeouts, connection timeouts, etc. Swapping to AsyncUDP using the exact same code as shown above with romkey's changes, resolved all of my issues. (Just using romkey's fixes did not fix the errors I was having with AsyncTCP.) I don't believe the issue is with AsyncTCP but with ESP32 libraries.
Either you should declare packetData to be of length len + 1 or your for loop should iterate until i < len. Because the index starts at 0, packetData[len] is actually byte len + 1, so you'll overwrite something random when you store something in packetData[len] if the array is only len chars long.That something random may be the pointer stored in packetBuf, which could easily cause heap corruption.
You should always use strncpy() and never strcpy(). Likewise use strncat() rather than strcat(). Don't depend on having done the math correctly or on sizes not changing as your code evolves. strncpy() and strncat() will guard against overflows. You'll need to pass a length into sensorData() to do that, but sensorData() shouldn't be making assumptions about the available length of retData.
Your test
if (clientData != NULL) {
will never fail because clientData is the address of array and cannot change. I'm not sure what you're trying to test for here but this if will always succeed.
You can just write:
char sensor[15] = "";
you don't need to explicitly assign a string with a null byte in it.
And
memcpy((char *)retData, "\0", 1);
is equivalent to
((char *)retData)[0] = '\0';
What's the point of declaring retData to be void * in the arguments to sensorData()? Your code starts out with it being a char* before calling sensorData() and uses it as a char* inside sensorData(). void * is meant to be an escape hatch for passing around pointers without worrying about their type. You don't need that here and end up needing to extra casts back to char* because of it. Just declare the argument to be char* and don't worry about casting it again.
You didn't share the code that calls handleData() so there may well be issues outside of these functions.
I've started learning C for Arduino for about 2 weeks. I have the following code and I don't understand how data is retrieved from function ReadLine. Also I don't understand how variable BufferCount affects the program and why it is used. I do know that it holds the number of digits the year have but that's about all I know about this variable.
From what I've learned so far a function is composed of:
function type specifier
function name
function arguments.
What I see in this program makes me think that the function can also return values using the argument part. I always thought that a function can only return a value that is the same type (int, boolean ...) as the type specifier.
void setup() {
Serial.begin(9600);
}
void loop() {
if (Serial.avaible() > 0) {
int bufferCount;
int year;
char myData[20];
bufferCount = ReadLine (myData);
year = atoi(myData); //convert string to int
Serial.print("Year: ");
Serial.print(year);
if (IsLeapYear(year)) {
Serial.print(" is ");
} else {
Serial.print(" is not ");
}
Serial.println("a leap year");
}
}
int IsLeapYear(int yr) {
if (yr % 4 == 0 && yr % 100 != 0 || yr % 400 == 0) {
return 1; //it's a leap year
} else {
return 0;
}
}
int ReadLine (char str[]) {
char c;
int index = 0;
while (true) {
if (Serial.available() > 0) {
c = Serial.read();
if (c != '\n') {
str[index++] = c;
} else {
str[index] = '\0'; //null termination character
break;
}
}
}
return index;
}
The fundamental concept you are missing is pointers. In the case of a function like isLeapYear there, you'd be right about that parameter. It is just a copy of the data from whatever variable was passed in when the function gets called.
But with ReadLine things are different. ReadLine is getting a pointer to a char array. A pointer is a special kind of variable that holds the memory address of another variable. And it is true that in this case you are getting a local copy of the pointer, but it still points to the same location in memory. And during the function, data is copied not into the variable str, but to the memory location it points to. Since that is a memory location that belongs to a variable in the scope of the calling function, that actual variable's value will be changed. You've written over it in memory.
My chip just stop doing anything. sometimes it prints good results, sometimes its not, i just cant understand whats wrong with this code( and generally any time you using Strings it happens )
void ParseGetRequest(char* data)
{
String parseGET=data;
String from="GET /";
String to="HTTP";
int ind1 = parseGET.indexOf(from);
int ind2 = parseGET.indexOf(to);
parseGET=parseGET.substring(ind1+from.length(), ind2-1);
strcpy(data, parseGET.c_str () );
}
And calling it with :
void readWifDataAsSever(char* reqData)
{
uint8_t buffer[128] = {0};
uint8_t mux_id;
uint32_t len = wifi.recv(&mux_id, buffer, sizeof(buffer), 100);
char serverData[100]={0};
if (len > 0)
{
for(uint32_t i = 0; i < len; i++)
serverData[i]=(char)buffer[i];
ParseGetRequest( serverData ); ///****** the call
Serial.println(serverData); // prints only part of the values
//here the chip just freeze and stop the main loop
NULL termination !!!!
serverData[len ] = '\0';