No nodes or topics visible using ESP32 and WiFi transport #139
Comments
|
I run a lot of esp32 with wifi transport. Check my wiki. You may try out with a bare esp32 module. |
|
I can get to the part where the esp32 connects to the Micro-ROS agent, it's just from that point on I can't seem to view any topics or nodes. |
|
There is no need to do these. Just use the default will work. X Install rmw_microxrcedds as well as the e-Prosima Micro-XRCE-DDS X Set the RMW_IMPLEMENTATION to rmw_microxrcedds X Changing the domain id settings : removing domain id / setting domain id = 0 |
|
May I suggest try out my code to get a real sense of micro-ros connection test? |
Oh ok, would I also need to build the firmware for the host? |
Sure I will try this out |
|
There are some useless codes in your program. The set_microros_wifi_transports will setup wifi. Those delay() are not needed. It seems you make it too complicated with class. It is better to keep the code clean. If you want to start from scratch. You may start with a publish int example with wifi transport. |
|
cyclonedds does not work well with micro-ROS. Only basic publish/subscribe will work. But services and actions are not supported. |
|
With some minor modification to platformio, this will be a good start. |
Yeah I changed it to rmw_microxrcedds |
The reason I wanted to do it with a class is because I am trying to integrate this code with a control system which will eventually be running on the esp32, so I felt it would de-clutter the code. Im assuming have these functionalities within a class won't affect the actual performance. |
|
You may skip the step of platfromio and linorobot2 installation, as you have installed micro_ros_setup. Modify the wifi keys and agent IP in config/custom/esp32_wifi_config.h Then it will try to connect to the agent. |
No. |
No. We would just use the default, fastrstp. |
No really impact to performance. It is the development process. For something new to you, it is better to start from basic. Build some, test some. Build some, test some. Step by step. Beside, sometime you will need to look into code of the upstream functions you called. |
Ah I see, I am kind of running tight on time for the project so I am trying to get it done ASAP, which is why I am trying to integrate it from the getgo |
|
Just opposite, it proved to take more time when you wrote a lot and found it took more time to debug. :) The better way is, find some working examples. Try out and modify to fit your purpose. Just my 2 cents. |
This was where I started. |
Very true, I am currently setting up the project that you recommended. |
|
For the config of the esp32 with wifi, if I am not planning on using the LiDAR, I can just comment out its' config right? |
|
I started from the publish int example. I built an esp32 wifi robot similar to yours. Later, I found the linorobto2 project, which has some interesting features. I ported my code to it. I added more devices and test programs. I think it is easy to expand to fit your purpose. I have tested service and action server. It should work for bigger projects. |
There is no need to change the other setting for the first connection test. Only change the wifi key and agent IP. You will change the other setting after the connection test success. |
|
I wrote a workflow in my wiki. These are the steps I used to build new robots. |
|
The very first step starts with a single bare esp32 module before wiring and building the robot. Try connect it to micro-ros. Make sure all the topics echoed and Hz rates are correct. |
After the first connection test success, you may comment out the Lidar setting. |
|
Hi, So I have been playing around and eventually got it working. I could see the topics and nodes as expected for my ESP program. Yet for some reason ability to see topics and nodes isn't consistent. I have been using the docker image with a ROS2 installation as provided on the tutorial page on the Micro-ROS website. When I run the micro ros agent with -v6 flag I can see data going from client to agent and all the topics, subscribers and so on are created fine, yet ros2 topic list only shows the default topics. To make sure its not an issue with my code, I ran the ping pong demo.
Source workspace and run agent
Source Workspace and run rclc ping pong demo
Established connection?
Not topics or nodes :(
|
|
I do not use docker. There is no need for rmw_microxrcedds. It is used for testing on PC, not for esp32. My suggestion is my wiki. You may test it with a bare esp32 wroom. Just connect with a USB cable. No other wiring is needed. |
|
Install the software Build, upload and test micro-ros wifi connection. Check topics |
Hi, I was able to get it working properly now. I think there is something wrong with my installation so I was using my friends machine, but that's beside the point. I am using topic based ros2 control to send joint commands to the ESP32 over topics to control the wheels, and I publish wheel states back to the joint state broadcaster via topics too. I am having a slight issue though. When publishing joint commands, I can see the data being published as I set the command velocities using the teleop_twist_keyboard. Yet the latency in receiving those commands on the esp32 seems to be crazy high, with some commands not being received or being received minutes later. This is the rqt_graph of the system for reference. The /joint_ctrl topic is the one the esp32 subscribes to for the wheel commands. I have a feeling its' due to publishing quite a bit of info on the joint states, IMU data and battery state whilst also trying to read the joint_ctrl topic, I am not quite sure as I am not too familiar with how the executor works with the timer and subscribers. I have attached the code below for the main file and the actual publish/subscribe callbacks : //Interrupt Service Routine for motor update
//Note: ESP32 doesn't support floating point calculations in an ISR
bool TimerHandler(void * timerNo)
{
static bool toggle = false;
//Update the stepper motors
left_motor.runStepper();
right_motor.runStepper();
//Indicate that the ISR is running
digitalWrite(TOGGLE_PIN,toggle);
toggle = !toggle;
return true;
}
void setup()
{
Serial.begin(115200);
pinMode(TOGGLE_PIN,OUTPUT);
left_motor_speed = 0.0;
right_motor_speed = 0.0;
// IMU Setup and WiFi Setup Code ...
//Attach motor update ISR to timer to run every STEPPER_INTERVAL_US μs
if (!ITimer.attachInterruptInterval(STEPPER_INTERVAL_US, TimerHandler))
{
Serial.println("Failed to start stepper interrupt");
while (1) delay(10);
}
Serial.println("Initialised Interrupt for Stepper");
//Enable the stepper motor drivers
pinMode(STEPPER_EN,OUTPUT);
digitalWrite(STEPPER_EN, false);
//Setting accelereration allows the stepper motors to move
left_motor.setAccelerationRad(30);
right_motor.setAccelerationRad(30);
// use_kinematic_control tells the system to listen on the \joint_ctrl topic for wheel commands rather than \cmd_vel
bool use_kinematic_control = true;
espRosAgent.Init(use_kinematic_control);
const unsigned int timer_timeout = 1000;
RCSOFTCHECK(rclc_timer_init_default(
&espRosAgent.timer,
&espRosAgent.support,
RCL_MS_TO_NS(timer_timeout),
timer_callback),
"Init Timer");
RCSOFTCHECK(rclc_executor_add_subscription(
&espRosAgent.executor,
&espRosAgent._kinematic_cmd_vel_sub, &espRosAgent._kinematic_cmd_msg,
&kinematic_cmd_vel_callback, ON_NEW_DATA),
"Create Kinematic Cmd Subscription");
}
oid loop()
{
//static unsigned long printTimer = 0; //time of the next print
static unsigned long loopTimer = 0; //time of the next control update
/*if (Serial.available() > 0)
{
String command = Serial.readStringUntil('\n'); // Read the command until newline
command.trim(); // Remove any whitespace or newline characters
if (command == "q")
{
Serial.println("");
Serial.println("------ Shutdown command received. Exiting... -------");
Serial.println("");
cleanup(); // Call the cleanup function
while(true); // Optionally, enter an infinite loop to stop further execution
}
}*/
if (millis() > loopTimer)
{
loopTimer += LOOP_INTERVAL;
// Get data
imu.getEvent(&imu_data.accel, &imu_data.gyro, &imu_data.temp);
motor_data.left_pos = left_motor.getPositionRad();
motor_data.left_speed = left_motor.getSpeedRad();
motor_data.right_pos = right_motor.getPositionRad();
motor_data.right_speed = right_motor.getSpeedRad();
//PID Loop : Uses data on /cmd_vel topic (in this case disabled)
if (!espRosAgent._kin_en)
{
accelTilt = imu_data.accel.acceleration.z/9.67 - 0.09;
gyroRate = imu_data.gyro.gyro.y;
tilt = CompFilter(accelTilt, gyroRate, alpha, tilt);
error = setpoint - tilt;
imu_data.accel.acceleration.y = tilt;
integral += error *dt;
PIDout = error * Kp - gyroRate*Kd + integral * Ki;
left_motor.setAccelerationRad(PIDout);
right_motor.setAccelerationRad(PIDout);
if (PIDout < 0)
{
left_motor.setTargetSpeedRad(-20);
right_motor.setTargetSpeedRad(-20);
}
else
{
left_motor.setTargetSpeedRad(20);
right_motor.setTargetSpeedRad(20);
}
PreviousError = error;
}
else
{
//left_motor_speed is just a global variable to hold target speed obtained from topic
left_motor.setTargetSpeedRad(left_motor_speed);
right_motor.setTargetSpeedRad(right_motor_speed);
}
}
rclc_executor_spin_some(&espRosAgent.executor, RCL_MS_TO_NS(100));
}
These are some relevant functions in the ROSComm class void ROSComm::Init(bool use_kinematic_control)
{
Serial.println("Setting micro-ROS wifi transports");
_kin_en = use_kinematic_control;
set_microros_wifi_transports(_ssid, _pswd, _agent_ip, _agent_port);
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
allocator = rcl_get_default_allocator();
RCSOFTCHECK(rcl_init_options_init(
&init_options,
allocator),
"Init RCL Options");
// Initialize rclc support object
RCSOFTCHECK(rclc_support_init(&support, 0, NULL, &allocator), "Init Default SUpport");
// Init node with configured support object
RCSOFTCHECK(rclc_node_init_default(
&node,
"esp_node", "",
&support),
"Init ESP Node");
CreatePublishers();
CreateSubscribers();
InitMessages();
unsigned int num_handles = 2;
RCSOFTCHECK(rclc_executor_init(
&executor,
&support.context,
num_handles,
&allocator),
"Init Executor");
}
void ROSComm::KinematicCommandCallback(const void *kin_cmd_vel_recv, double *left_motor_vel, double *right_motor_vel)
{
_kinematic_cmd_msg = *(sensor_msgs__msg__JointState *) kin_cmd_vel_recv;
*left_motor_vel = _kinematic_cmd_msg.velocity.data[0];
*right_motor_vel = _kinematic_cmd_msg.velocity.data[1];
}
void ROSComm::PublishCallback(motor_data_t *motor_data, imu_data_t *imu_data, int BattLevel, int BattPower)
{
getData(motor_data, imu_data, BattLevel, BattPower);
rcl_ret_t rc;
rc += rcl_publish(&_joint_state_pub, &_joint_states_msg, NULL);
rc += rcl_publish(&_imu_pub, &_imu_msg, NULL);
rc += rcl_publish(&_batt_lvl_pub, &_battery_lvl_msg, NULL);
rc += rcl_publish(&_batt_pwr_pub, &_battery_pwr_msg, NULL);
}
Could discrepancy between the data published onto the /joint_ctrl topic and the data read from it on the esp32 be related to the executor or timer timeout? I know this issue was raised for a totally different reason, but I was hoping you could give some insight given your vast experience with ESP32s. This is for a project I am making so unfortunately I couldn't resort to your repo. Thank you |
|
I didn't mean that you should use the linorobot_hardware for your project. I suggested that you tried out the firmware to get the idea how it works. Then you can compare the codes and resolve your problem. I have my own projects and you will need to debug on your own. |
Issue template
Steps to reproduce the issue
Install ROS2 humble as per the typical user guide.
Install micro-ros-platformio
Install micro_ros_setup as given on the repo
Install rmw_microxrcedds as well as the e-Prosima Micro-XRCE-DDS
Set the RMW_IMPLEMENTATION to rmw_microxrcedds
Set DOMAIN_ID to 0 on host
run the micro ros agent over udp with port 8888
Flash the following code onto ESP32 :
Main File
ROSComm Class
Expected behavior
Successful connection between micro-ROS agent and ESP32
ros2 topic list prints out topics relating to the right and left wheel
Actual behavior
Successful connection between micro-ROS agent and ESP32
Topics, Publishers and Data writers created
ESP Serial Confirms successful initialisation
Apparent Successful Data Exchange
At this point, when running ros2 topic list, the only visible topics are the typical rosout and parameter events.
I have been playing around with this for days, I have tried :
By this point I have lost most hope and have no idea what the underlying issue is. I have searched online and most people were able to solve it by either using a common domain id or not using one at all.
I will love you with all my heart if you can help out <3
Additional information
I also noticed a weird thing happening when I restart the ROS2 daemon. Although this may be relevant to a different issue, maybe this could explain the missing nodes/topics :
The text was updated successfully, but these errors were encountered: