Can i make this project using Arduino mega2560 and LidarA1M8 instead of Teensy and A2M8?

[thanhtruongphan]

Hi everyone, i'am a newbie ROS, i want to make a ROS mobile robot myself, this project is very helpful but I only have RPi 4B, Arduino Mega2560, Lidar A1M8 now.

Can i do this projet with these?
if yes,
how can I do?

Thank you.

[fjp]

Hi @thanhtruongphan thanks for the question.

Yes you can also use A1M8, there is also a mount available for REMO robot:

You can get it from here or by becoming a sponsor you will receive all the latest updates.

For the lidar ROS node check out: https://github.com/Slamtec/rplidar_ros it also supports A1 lidars.

And instead of flashing the base_controller software onto the teensy you can go for Arudino. The documentation explains the process for teensy but it should be quite similar for Arduino https://ros-mobile-robots.com/processing_units/teensy-mcu/#base-controller

[thanhtruongphan]

yeah!

thanks for your reply. I will see the documentation explains that you've attached.

beside, I see the Part List, it has a Groove I2C driver and a Boosch IMU, but I certainly don't have these.

all i have are

• raspi4 4GB
• arduino mega 2560
• pi camera v2
• IMU name's MPU6050
• Lidar A1M8
• 2 motor with encoder
• power 12v
• power 5v for rasp,
• L298 motor driver

I had build those to a diff-drive robot base, hardware is simple and no problem.
Can I apply your software-source-code to my hardware? If not, I want to change something to cooperate with my ideas, but I don't know how to do...

and, I really want to know each part of your source, but there are very much documentations that make me confused.

can you give me some instruction to learn?

Thanks a lot...!!!

[fjp]

Motor Driver

The Groove I2c driver was used for the first version of diffbot software were most of the motor control was done on a high level running on the single board computer and only the code to count the encoder ticks was running on the microcontroller (see this deprecated encoders folder). With the latest state of diffbot I suggest to run the motor driver software on the microcontroller board (Arduino in your case) and have a look into the base_controller low level scripts in the diffbot_base package.

REMO robot, the latest version, uses Adafruit feather wing motor driver which implements the simple motor_controller_interface. In case you want to use the L298 motor driver I suggest you create a new cpp and header file similar to the one as Adafruit feather wing motor driver. Once that is done you can use it in the main.cpp of the base_controller node.

IMU

In the beginning you don't need the IMU. Everything will work without IMU and run fine with the encoders. When everything works with the encoders as localization input you can add the IMU which does improve the localization, because the encoders can be inaccurate. For the IMU support you would need to look into #60 and #37. Still needs some testing to be integrated into the source code as a new ros package.

[thanhtruongphan]

hiii Mr. Franz,

I just launch with "roslaunch diffbot_bringup bringup_with_laser.launch"

everything is okay, but It has a problem, (as the picture i attached)

I have tried to change baudrate to 57600, 9600 but it still error.

help me :D

thank u so much!

[thanhtruongphan]

as you can see, i try to change Velocities but my motor still not move

[fjp]

To me it seems the issue is on the mcu board. But to verify let's try to get some more log outputs and understand where in the control flow you get stuck (cmd_vel -> code running on RPi -> high level ros control hardware interface (write method) -> publish motor topics -> mcu board -> low level rosserial base_controller code -> subscriber of the wheel command -> translate to motor commands -> motor driver interaction.

You are interested in this pub_wheel_cmd_velocities_, which should publish messages to wheel_cmd_velocities topic. I suggest you also use rostopic echo to see if you get any output on this topic when you try to command the robot around (using rqt_robot_steering or keyboard_teleop). If that works the problem has to be on the mcu board.

To investigate the mcu board I would add more logging on to the base_controller code that is running on the mcu. For example add some more logging similar to this https://github.com/ros-mobile-robots/diffbot/blob/noetic-devel/diffbot_base/scripts/base_controller/lib/base_controller/base_controller.h#L594. You can try adding some more log messages to the subscriber code of wheel_cmd_velocities (for example a simple output like "received wheel_cmd_velocities message"). When you see this trying to move around then there is either a logic problem in how you interact with your motor driver or some update rate issues.

Just a side note: you should see more debug messages from the ros control hardware interface (running on the SBC) when you enable logging here: https://github.com/ros-mobile-robots/diffbot/blob/noetic-devel/diffbot_base/config/base.yaml#L11. But it shouldn't be required, you can also add more relevant logging to the code in the write() method without enabling this flag

[thanhtruongphan]

Hey Franz, have a nice day! 😄

I'm still not fix it correctly.

Here's the code base_controller I've modified: Github

If you have free time, pls check it with me. Thank you so much!!

And here is the problem now:

• I run command roslaunch diffbot_bringup bringup.launch and result is all like:

the result [INFO] [1729910983.080179]: encoder_ticks_:01980 is because i code the log_info and in mainpp file with getRPM()

And when I turn round the real-wheel, it [ERROR] [1729910996.407401]: Lost sync with device, restarting... and cannot continue communicate with ROS.

--
about control PWM, if I uncomment the two lines on the mainpp.cpp code,
it's OK, the left wheel turn with 750/1000, and the right wheel turn with 250/1000 correctly (the max PWM that I configed from hardware is 1kHz). I had test both encoders and PWM on MCU and it's ok. Maybe the base_controller code that I modified have problem...

The MCU that I use is STM32F411CE with 128kb RAM, 512kb Flash and 100MHz link

It's quite large than Teensy 3.2 (of course It's still small than Teensy 4.0 very much),

Could you have and ideas for me...?
Thank you 🥹

[fjp]

Hi @thanhtruongphan you might have already found the following suggestions but let me share them here as well:

This error can mean so many different things. First of all you should check that the topic names and message type actually line up. Then check the baud rate. Also this might happen if any of the subscribers or publishers stop existing during the connection. So make sure you never go out of scope. As in you can't have a subscriber stop existing after you send the message.

source: https://www.reddit.com/r/ROS/comments/18m4k36/error_lost_sync_with_device_restarting/

• Maybe you are missing the custom message generation step? https://ros-mobile-robots.com/diffbot_msgs/#rosserial
• Someone mentioned they had to call nh.negotiateTopics(); to fix the issue Tried to publish before configured topic id ...

source: https://answers.ros.org/question/232355/tried-to-publish-before-configured-topic-id-125/

Code review

I've had a quick look at your code and I am not sure is if your setup is correct. The PWM and encoder starts are inside the main loop, which means they're being repeatedly initialized every iteration. Maybe these should be moved outside the while loop, into the setup section? Does it maybe help if you restructure your main.cpp something like this:

int main(void)
{
  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/
  HAL_Init();
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM5_Init();

  /* USER CODE BEGIN 2 */
  // Move timer initializations here, before setup()
  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
  HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL); // Start encoder 1
  HAL_TIM_Encoder_Start(&htim5, TIM_CHANNEL_ALL); // Start encoder 2
  
  // Start timers with interrupts
  HAL_TIM_Base_Start_IT(&htim3);
  HAL_TIM_Base_Start_IT(&htim4);

  setup();
  /* USER CODE END 2 */

  /* Infinite loop */
  while (1)
  {
    loop();
  }
  /* USER CODE END 3 */
}

The "Tried to publish before configured topic id" error typically occurs when you're trying to publish messages before the ROS node has fully initialized and established communication with the rosserial server. This often happens in conjunction with sync issues.
Here's what I recommend trying, in order:

1. First, try the code rearrangement I suggested above - moving the timer initializations outside the main loop. This could help with the sync issues.
2. If you're still seeing issues, modify your setup() to ensure proper ROS initialization:

void setup() {
  nh.initNode();
  // Add delay after node init
  HAL_Delay(1000);  // Give ROS time to initialize
  
  // Initialize all your publishers/subscribers
  nh.advertise(your_publisher);
  // Add another small delay
  HAL_Delay(100);
}

void loop() {
  // Your publishing code
  your_publisher.publish(&your_message);
  
  nh.spinOnce();
  HAL_Delay(10);  // Adjust this delay as needed
}

3. Common causes of both issues:
   Node not fully initialized before publishing
   Incorrect baudrate settings
   Topics being published too quickly
   Memory issues (if you have many publishers/subscribers)

Looking at your code in mainpp.cpp, potential causes for both the sync issues and the "publish before configured" error could be the following:

• Timing Issues:

void setup()
{
    base_controller.setup();
    base_controller.init();
    nh.loginfo("Setup finished");
    // Missing initialization delay and explicit node initialization
}

Try modifying the setup():

void setup()
{
    nh.initNode();  // Explicitly initialize the node
    HAL_Delay(1000);  // Give time for node initialization
    
    base_controller.setup();
    base_controller.init();
    
    HAL_Delay(100);  // Give time for topics to register
    nh.loginfo("Setup finished");
}

• Loop Timing:
  Your loop contains several blocks of time-critical code but the delay might be too long:

    nh.spinOnce();
    HAL_Delay(100);  // 100ms might be too long

Try reducing this to:

```cpp
    nh.spinOnce();
    HAL_Delay(10);  // Try a shorter delay

• Add error checking in the setup:

void setup()
{
    nh.initNode();
    HAL_Delay(1000);
    
    if (!nh.connected()) {
        nh.logfatal("Failed to connect to ROS master");
        return;
    }
    
    base_controller.setup();
    base_controller.init();
    
    HAL_Delay(100);
    nh.loginfo("Setup finished");
}

• Consider checking connection status in the loop:

void loop()
{
    if (!nh.connected()) {
        nh.spinOnce();
        HAL_Delay(1000);
        return;
    }
    
    // Rest of your existing loop code...
}

• Memory Usage:
  You have several objects (controllers, encoders) that might be publishing topics. Try also not exceeding the maximum number of publishers/subscribers for your MCU's memory constraints.

• base controller code
  The base controller setup looks generally ok, but there are a few potential improvements we could try:

template <typename L298MotorController>
void diffbot::BaseController<L298MotorController>::setup()
{
    nh_.initNode();
    // Add delay after node init
    HAL_Delay(500);

    // Memory allocation checks
    if (!msg_measured_joint_states_.position || !msg_measured_joint_states_.velocity) {
        nh_.logfatal("Failed to allocate memory for joint states");
        return;
    }

    // Setup publishers first
    nh_.advertise(pub_encoders_);
    HAL_Delay(50);  // Small delay between publisher setups
    
    msg_measured_joint_states_.position = (double*)malloc(sizeof(double) * 2);
    msg_measured_joint_states_.position_length = 2;
    msg_measured_joint_states_.velocity = (double*)malloc(sizeof(double) * 2);
    msg_measured_joint_states_.velocity_length = 2;
    nh_.advertise(pub_measured_joint_states_);
    HAL_Delay(50);

    // Then setup subscribers
    nh_.subscribe(sub_wheel_cmd_velocities_);
    nh_.subscribe(sub_reset_encoders_);
    nh_.subscribe(sub_pid_left_);
    nh_.subscribe(sub_pid_right_);

    // Connection check with timeout
    uint32_t connection_timeout = HAL_GetTick() + 5000;  // 5 second timeout
    while (!nh_.connected())
    {
        nh_.spinOnce();
        HAL_Delay(10);
        
        if (HAL_GetTick() > connection_timeout) {
            nh_.logfatal("Failed to connect to ROS master within timeout");
            return;
        }
    }
    
    nh_.loginfo("Base controller setup completed successfully");
}

Key improvements:

• Added delays between initialization steps
• Added memory allocation checks
• Separated publisher and subscriber setup
• Added timeout to the connection check
• Added success log message

Two things to check:

• Are you freeing the allocated memory somewhere when needed?
• What is your UART baudrate set to? This needs to match on both the ROS and mcu side.

Finally, adding logging statements in the form nh.loginfo("helpful message"); to appropriate places will help identify:

• If the setup process completes successfully
• Where in the initialization process things might be failing
• If and when the ROS connection is lost
• When control cycles are happening
• Encoder readings
• Don't log too frequently as it can affect performance

Start with a simple example

If you haven't done it already I'd start with a minimal rosserial example on your STM32 mcu. Maybe getting a LED to switch on/off through a simple rostopic: http://wiki.ros.org/rosserial_arduino/Tutorials/Blink. Just to see if the basic setup works and then go back to the much harder to setup base_controller code.

Good luck and I hope some of these suggestions will help you 🤞

[thanhtruongphan]

because of my STM32, I don't know, I tested separately with rosserial_stm32 > publisher encoder ticks, It doesn't run smoothly, maybe all the error come from my stm32...
I just orderd a teensy board, I'll use it :'(