@JoshNewans I looked a bit more into it and it seems it is due to CPU overload causing missed deadlines and loop rates. But it’s not just Nav2; the controller_manager is using 30% of it! I’m not using a dev machine, so everything is in my Raspberry Pi 4B, but is this even normal? Shouldn’t the 8GB RAM Pi be powerful enough to run at least decently?
Hi Josh and all,
I followed this great video to get my own diffdrive robot to work with ros2.
I got quite far, but atm I am hitting a wall.
What I can is get the motors to work with with a serial terminal and sending commands. So I know that part functions.
I can get
I have the diffbot_base_controller example running (just changed the serial port settings)
bert@trout:~/lab/ros2/src/diffdrive_arduino-humble$ ros2 node list
/controller_manager
/diffbot_base_controller
/joint_state_broadcaster
/robot_state_publisher
/rviz2
/transform_listener_impl_582122448330
bert@trout:~/lab/ros2/src/diffdrive_arduino-humble$ ros2 topic list
/clicked_point
/diagnostics
/diffbot_base_controller/cmd_vel
/diffbot_base_controller/odom
/diffbot_base_controller/transition_event
/dynamic_joint_states
/goal_pose
/initialpose
/joint_state_broadcaster/transition_event
/joint_states
/parameter_events
/robot_description
/rosout
/tf
/tf_static
But
ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args --remap /cmd_vel:=/diffbot_base_controller/cmd_vel
doesn’t move anyrhing. neither the physical robot, nore in Rviz. I don’t know how to verify if the remapping is correct. But as there is no other cmd_vel in the topic list above, I think this is correct, right?
I would like to bisect the problem. I put some print statements in diffbot_system.cpp and arduino_coms.hpp. It doesn’t look like they are triggered from teleop_twist_keyboard. So is my remapping wrong? Any advice is appreciated. A clue as to what I’m doing wrong or a direction for furhter searching.
Kind regards,
Bert
Answering my own question.
It turned out I ran into the issue that diffbot_base_controller/cmd_vel expects TwistStamped format, while Teleop_twist_keboard uses unstamped Twist format.
I found the twist_stamper Advanced Teleop | Articulated Robotics created by Josh. This made things work.
I read there is also a way to change the teleop_twist_keyboard output directly to stamped these days, but I haven’t got that working yet.
Bert
I have spent the last 5 days trying to get my physical robot running for the first time with ROS2 code after following Josh’s video series. It is not working and need help debugging the issue. Any help you can offer is very much appreciated! Does anything jump out as being a problem?
Environment:
- Raspberry Pi 4 with 8Gb running Ubuntu Noble (24.0.x).
- ROS2 Jazzy
- Using Arduino Nano per the motor control video. Running Josh’s serial motor demo works as expected
- Building my workspace with diffdrive_Arduino from Redstone Github and Serial from Josh’s Github is successful.
- Workspace is deployed and running on the Pi. In RQT I see the topics and nodes.
Here is what I see when running the launch file. It looks like a clean start to me:
tom@pi4-u24:~/robby_ws$ source install/setup.bash
tom@pi4-u24:~/robby_ws$ ros2 launch robby_beta launch_robot.launch.py
[INFO] [launch]: All log files can be found below /home/tom/.ros/log/2025-02-24-20-58-06-873412-pi4-u24-3276
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [robot_state_publisher-1]: process started with pid [3295]
[INFO] [joy_node-2]: process started with pid [3296]
[INFO] [teleop_node-3]: process started with pid [3297]
[INFO] [twist_stamper-4]: process started with pid [3298]
[INFO] [twist_mux-5]: process started with pid [3299]
[robot_state_publisher-1] [INFO] [1740452288.127663346] [robot_state_publisher]: Robot initialized
[twist_mux-5] [INFO] [1740452288.160389120] [twist_mux]: Topic handler 'topics.joystick' subscribed to topic 'cmd_vel_joy': timeout = 0.500000s , priority = 100.
[teleop_node-3] [INFO] [1740452288.178560927] [TeleopTwistJoy]: Teleop enable button 6.
[teleop_node-3] [INFO] [1740452288.178747500] [TeleopTwistJoy]: Turbo on button 7.
[teleop_node-3] [INFO] [1740452288.178770056] [TeleopTwistJoy]: Linear axis x on 1 at scale 0.500000.
[teleop_node-3] [INFO] [1740452288.178795574] [TeleopTwistJoy]: Turbo for linear axis x is scale 1.000000.
[teleop_node-3] [INFO] [1740452288.178812797] [TeleopTwistJoy]: Angular axis yaw on 0 at scale 0.500000.
[teleop_node-3] [INFO] [1740452288.178829148] [TeleopTwistJoy]: Turbo for angular axis yaw is scale 1.000000.
[twist_mux-5] [INFO] [1740452288.179375295] [twist_mux]: Topic handler 'topics.navigation' subscribed to topic 'cmd_vel': timeout = 0.500000s , priority = 10.
[twist_mux-5] [INFO] [1740452288.182217877] [twist_mux]: Topic handler 'topics.tracker' subscribed to topic 'cmd_vel_tracker': timeout = 0.500000s , priority = 20.
[INFO] [ros2_control_node-6]: process started with pid [3441]
[INFO] [spawner-7]: process started with pid [3442]
[INFO] [spawner-8]: process started with pid [3443]
[ros2_control_node-6] [INFO] [1740452296.709686302] [controller_manager]: Subscribing to '/robot_description' topic for robot description.
[ros2_control_node-6] [INFO] [1740452296.716557760] [controller_manager]: update rate is 300 Hz
[ros2_control_node-6] [INFO] [1740452296.716708241] [controller_manager]: Spawning controller_manager RT thread with scheduler priority: 50
[ros2_control_node-6] [WARN] [1740452296.717865515] [controller_manager]: Could not enable FIFO RT scheduling policy: with error number <1>(Operation not permitted). See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details on how to enable realtime scheduling.
[ros2_control_node-6] [INFO] [1740452296.724590085] [controller_manager]: Received robot description from topic.
[ros2_control_node-6] [INFO] [1740452296.751359901] [controller_manager.resource_manager]: Loading hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452296.755942497] [controller_manager.resource_manager]: Loaded hardware 'RealRobot' from plugin 'diffdrive_arduino/DiffDriveArduino'
[ros2_control_node-6] [INFO] [1740452296.756177126] [controller_manager.resource_manager]: Initialize hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452296.760072557] [DiffDriveArduino]: Configuring...
[ros2_control_node-6] [INFO] [1740452296.772300534] [DiffDriveArduino]: Finished Configuration
[ros2_control_node-6] [INFO] [1740452296.772405811] [controller_manager.resource_manager]: Successful initialization of hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452296.773217568] [resource_manager]: 'configure' hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452296.773287827] [resource_manager]: Successful 'configure' of hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452296.773316345] [resource_manager]: 'activate' hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452296.773336271] [DiffDriveArduino]: Starting Controller...
[spawner-8] [INFO] [1740452297.912544176] [spawner_diff_cont]: waiting for service /controller_manager/list_controllers to become available...
[spawner-7] [INFO] [1740452298.556509321] [spawner_joint_broad]: waiting for service /controller_manager/list_controllers to become available...
[ros2_control_node-6] [INFO] [1740452298.775302844] [resource_manager]: Successful 'activate' of hardware 'RealRobot'
[ros2_control_node-6] [INFO] [1740452298.775423881] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services...
[ros2_control_node-6] [INFO] [1740452298.819089585] [controller_manager]: Loading controller : 'joint_broad' of type 'joint_state_broadcaster/JointStateBroadcaster'
[ros2_control_node-6] [INFO] [1740452298.819270881] [controller_manager]: Loading controller 'joint_broad'
[ros2_control_node-6] [INFO] [1740452298.839282221] [controller_manager]: Controller 'joint_broad' node arguments: --ros-args --params-file /tmp/launch_params_juxlxd5v --params-file /home/tom/robby_ws/install/robby_beta/share/robby_beta/config/my_controllers.yaml
[spawner-7] [INFO] [1740452298.949981497] [spawner_joint_broad]: Loaded joint_broad
[ros2_control_node-6] [INFO] [1740452298.953278097] [controller_manager]: Loading controller : 'diff_cont' of type 'diff_drive_controller/DiffDriveController'
[ros2_control_node-6] [INFO] [1740452298.953554077] [controller_manager]: Loading controller 'diff_cont'
[ros2_control_node-6] [INFO] [1740452298.972969753] [controller_manager]: Controller 'diff_cont' node arguments: --ros-args --params-file /tmp/launch_params_juxlxd5v --params-file /home/tom/robby_ws/install/robby_beta/share/robby_beta/config/my_controllers.yaml
[ros2_control_node-6] [INFO] [1740452299.143990679] [controller_manager]: Configuring controller: 'joint_broad'
[ros2_control_node-6] [INFO] [1740452299.144302141] [joint_broad]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-8] [INFO] [1740452299.146804632] [spawner_diff_cont]: Loaded diff_cont
[ros2_control_node-6] [INFO] [1740452299.189124787] [controller_manager]: Configuring controller: 'diff_cont'
[ros2_control_node-6] [WARN] [1740452299.189515452] [diff_cont]: [deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits to NAN
[ros2_control_node-6] [WARN] [1740452299.189669340] [diff_cont]: [deprecated] has_acceleration_limits parameter is deprecated, instead set the respective limits to NAN
[ros2_control_node-6] [WARN] [1740452299.189715951] [diff_cont]: [deprecated] has_velocity_limits parameter is deprecated, instead set the respective limits to NAN
[ros2_control_node-6] [WARN] [1740452299.189747525] [diff_cont]: [deprecated] has_acceleration_limits parameter is deprecated, instead set the respective limits to NAN
[ros2_control_node-6] [INFO] [1740452299.225667053] [controller_manager]: Activating controllers: [ joint_broad ]
[ros2_control_node-6] [INFO] [1740452299.242380645] [controller_manager]: Activating controllers: [ diff_cont ]
[spawner-7] [INFO] [1740452299.248575902] [spawner_joint_broad]: Configured and activated joint_broad
[spawner-8] [INFO] [1740452299.267758782] [spawner_diff_cont]: Configured and activated diff_cont
[INFO] [spawner-7]: process has finished cleanly [pid 3442]
[INFO] [spawner-8]: process has finished cleanly [pid 3443]
I do see these when running ros2 wtf:
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 45: UserWarning: Subscriber without publisher detected on /cmd_vel_tracker.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /diagnostics.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /diff_cont/odom.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /diff_cont/transition_event.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /diffbot_base_controller/cmd_vel.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /dynamic_joint_states.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /joint_broad/transition_event.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 45: UserWarning: Subscriber without publisher detected on /joy/set_feedback.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /tf.
/opt/ros/jazzy/lib/python3.12/site-packages/ros2doctor/api/topic.py: 42: UserWarning: Publisher without subscriber detected on /tf_static.
Checking the URDF is successfull:
t
om@pi4-u24:~/robby_ws$ check_urdf <(xacro src/robby_beta/description/robot.urdf.xacro)
robot name is: robby_beta
---------- Successfully Parsed XML ---------------
root Link: base_link has 3 child(ren)
child(1): chassis
child(1): acquisition
child(2): left_omni_wheel
child(3): right_omni_wheel
child(2): left_wheel
child(3): right_wheel
Some ros2 control results:
tom@pi4-u24:~/robby_ws$ ros2 control list_hardware_interfaces
command interfaces
diff_cont/angular/velocity [available] [unclaimed]
diff_cont/linear/velocity [available] [unclaimed]
left_wheel_joint/velocity [available] [claimed]
right_wheel_joint/velocity [available] [claimed]
state interfaces
left_wheel_joint/position
left_wheel_joint/velocity
right_wheel_joint/position
right_wheel_joint/velocity
tom@pi4-u24:~/robby_ws$ ros2 control list_controllers
diff_cont diff_drive_controller/DiffDriveController active
joint_broad joint_state_broadcaster/JointStateBroadcaster active
tom@pi4-u24:~/robby_ws$ ros2 control list_hardware_components
Hardware Component 1
name: RealRobot
type: system
plugin name: diffdrive_arduino/DiffDriveArduino
state: id=3 label=active
command interfaces
left_wheel_joint/velocity [available] [claimed]
right_wheel_joint/velocity [available] [claimed]
I have been troubleshooting my problem. I have one problem left, and I don’t see how the ROSArduinoBridge works for other people.
My problem: The ROSArduinoBridge sketch found at GitHub - joshnewans/ros_arduino_bridge: ROS + Arduino = Robot doesn’t work.
I added a section for my motor driver (Cytron MD10C) Cytron MD10C. Clean compile, double checked wiring. When I run miniterm using the m command (which is the command sent from diffdriveArduino), it starts the motors and rapidly escalates to the max PWM of 255. Sending an m 3 3 command, starts the motor slowly, but within 20 cycles it is at a pwm of 255. Sending an m 20 20 command gets to pwm of 255 in fewer cycles. The bigger the m command the fewer cycles to get to pwm 255, but any m command always ends up at pwm 255.
Checking the encoders by hand, I see the encoders count change as expected. My encoder is a 7 PPR which translates to 28 counts per motor revolution. With my 6:1 gearing on there that translates to 168 counts per one wheel revolution.
I put a serial.print on the input and the error and I see the error grow every cycle, but never correcting.
I don’t see where the pid stuff is working. What I don’t understand is how others are using the same code from Articulated Robotics and not running into the same issue.
Any thoughts are where to go from here?