ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org
Ask Your Question
0

Callback function not executed arduino code

asked 2021-12-01 20:55:52 -0600

FengHan gravatar image

updated 2021-12-04 08:26:58 -0600

Mike Scheutzow gravatar image

Hello all I have a simple Arduino code to control a servo. I used the following code for my teensy board. I just find I cannot publish the command from ros and send it to the teensy board. The communication between the teensy board and the servo is correct as I can directly send commands to the servo by seting some constant commands in the Arduino code.

Then I used a dummy topic to test whether the callback function to subscribe the command from ros is called or not (shown in the following code). The value of the dummy topic "truck/manual/throttle" is always zero. Clearly, spinonce() function doesn't call the callback function to get the command I sent otherwise the value should be 10. The following is the code. Can anyone help with this

#include <ros.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/Imu.h>
#include <std_srvs/SetBool.h>
#include <std_srvs/Empty.h>
#include <Encoder.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Servo.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include "TeensyThreads.h"
#define USE_TEENSY_HW_SERIAL
/****PIN CONNECTION MACROS****/
#define _SERVO 1
#define _ESC 3
/*******Reciever PWM range macros*********/
#define _REC_ESC_MIN 1065
#define _REC_ESC_MAX 2006
#define _ESC_NEUTRAL 1514
#define _SERVO_NEUTRAL 1610

/************STRUCT DEFINTIONS************/
std_msgs::Float32 manualThrottle;
std_msgs::Float32 cmdThrottle;

/*********PROTOTYPES**************/
void cmdThrottleCb(const std_msgs::Float32 &msg);

 /************VARIABLES********************/
Servo esc;
Servo steer;
ros::NodeHandle nh;

ros::Publisher pub_manual_throttle_command("truck/manual/throttle", &manualThrottle);
ros::Subscriber<std_msgs::Float32> sub_cmdThrottle("truck/cmd/throttle", &cmdThrottleCb);
/************************************************/

int Flag = 0;
int Cmd_esc = 0;

void setup(){
  esc.attach(_ESC);
  steer.attach(_SERVO);

  steer.writeMicroseconds(_SERVO_NEUTRAL); //send neutral commands for safety
  esc.writeMicroseconds(_ESC_NEUTRAL); 

  nh.initNode();
  nh.advertise(pub_manual_throttle_command);  
  nh.subscribe(sub_cmdThrottle); //control command 
  delay(20); 
}

void loop()
{
  manualThrottle.data=Flag; // publish this data to test callback function
  pub_manual_throttle_command.publish(&manualThrottle);
  esc.writeMicroseconds(Cmd_esc);  
  nh.spinOnce();
  delay(10);
}

void cmdThrottleCb(const std_msgs::Float32 &msg){
  Cmd_esc=msg.data;
  Flag=10;// test if this function is called
}
edit retag flag offensive close merge delete

Comments

Just two quick observations. You send a float but Cmd_esc is of type int. I have seen issues where arduino basic types do not match "ros"/PC types(there were different "double" types I think), causing trouble/errors. Iam not sure how well implicit casting works here. Way back I also seen that declaring a function below void loop() didnt work for some code. I dont get why there is a prototype of cmdThrottleCb at the top and the implementation at the bottom. Get rid of the prototype?

Dragonslayer gravatar image Dragonslayer  ( 2021-12-02 06:06:08 -0600 )edit

Thanks. I revised my code, just change "Flag" to float type. The "cmdThrottleCb" function is used to create the publisher:

ros::Publisher pub_manual_throttle_command("truck/manual/throttle", &manualThrottle);

But this still doesn't work, the teensy 4.0 microcontroller cannot receive the command and the servo doesn't rotate correspondingly.

FengHan gravatar image FengHan  ( 2021-12-02 09:59:36 -0600 )edit

I would still try to implement the cmdthrottlecb() up where the prototype is. All your linked code does it this way as well. I would also build a led on/off trigger into the callback. I checked some of my old code and this should work. What does your rosserial launchfile look like on the PC side? Also regarding "#define USE_TEENSY_HW_SERIAL", does your libraries etc comply with this and is 4.0 adapted? How do you connect to the PC? Did any other rosserial scetch work on this teensy 4.0 for you?

Dragonslayer gravatar image Dragonslayer  ( 2021-12-02 13:20:45 -0600 )edit

Thanks. I tried what you said. It still doesn't work. I tried to publish command from the terminal for the topic truck/cmd/throttle as

rostopic pub truck/cmd/throttle std_msgs/float32 "data: 10.0"

The terminal then displays the following info

publishing and latching message

If I didn't Ctr+c, the info is always there.

FengHan gravatar image FengHan  ( 2021-12-02 20:35:48 -0600 )edit

You could "rostopic info" and "rostopic hz" the topics to at least see whats going on the PC side. But whats with my questions about teensy serial setup? I had issues with it way back. Did you use the LED to confirm that anything ever reaches the teensy via serial? How do you connect USB? Did this setup ever work with rosserial?

Dragonslayer gravatar image Dragonslayer  ( 2021-12-03 06:08:45 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-12-02 06:31:13 -0600

Mike Scheutzow gravatar image

updated 2021-12-02 06:47:23 -0600

  1. You can not assume that a single spinOnce() will handle all the background activity that ros has pending.

  2. I have never seen an arduino run "full size" ros (hint: that requires a network stack.) Instead, what people do is split the implementation into two pieces using rosserial. The arduino side runs a cut-down subset of ros functions, optimized for the limited abilities of this cpu, and the arduino app you end up with is NOT a real ros node, because it requires the rosserial-master to do any work. You have to compile this arduino application using a special procedure.

See:

http://wiki.ros.org/rosserial

http://wiki.ros.org/rosserial_embedde...

Using rosserial can be a little confusing, so it's not a good place for someone new to ros to start with. The experience will go much easier if you learn the ros APIs on a desktop pc first.

edit flag offensive delete link more

Comments

Thanks for your answer. My system contains a Linux computer with ros, a teensy 4.0 microcontroller, and a servo. The command for the servo is sent from ros to teensy and teensy will do the low-level control.

The code I provided above in my question is loaded into teensy 4.0. But I just find that the command sent from ros cannot be received by the subscriber. Then I used the above code to test and find the callback function in teensy is not executed.

We used a similar code before to control three step-motors (Linux ros+teensy+motors). It worked. Also you can see a similar code in GitHub: https://gist.github.com/jones2126/138... and https://github.com/nr-parikh/rosseria...

I test the second code and it still doesn't work (still defined a dummy topic to test callback function).

FengHan gravatar image FengHan  ( 2021-12-02 10:09:17 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-12-01 20:55:52 -0600

Seen: 241 times

Last updated: Dec 02 '21