search

sbgisen / vesc

VESC Interface for ROS
Apache License 2.0
41 stars 33 forks source link

ros2_control for vesc motor controller not working with two vescs #90

Open Persius48 opened 10 months ago

Persius48 commented 10 months ago

I am currently working on a project to make a four-wheeled differential drive robot using 4 VESCs with ROS2 as the means of communication. Two vesc will be masters and the rest two will be slaves. I used the vesc_hw_interface with humble-devel for one VESC and it was okay. but when I tried to use two VESCs with some modification in the ros2_control_xacro by including two hardware and two joints, there was an error like the following. I also want to add that I have been using vesc driver packages from f1tenth as it also has the IMU messages and have edited the codes in vesc_hw_interface accordingly to use that.

Invalid checksum
[ros2_control_node-1] [ERROR] [1693755709.123338959] 
[VescHwInterface]: Out-of-sync with VESC, discarding 49 bytes.
[ros2_control_node-1] [ERROR] [1693755709.128419540] 
[VescHwInterface]: Invalid end-of-frame character
[ros2_control_node-1] [ERROR] [1693755709.128438065] 
[VescHwInterface]: Invalid payload length
[ros2_control_node-1] [ERROR] [1693755709.128448735] 
[VescHwInterface]: Out-of-sync with VESC, discarding 21 bytes.
[ros2_control_node-1] [ERROR] [1693755709.163161253] 
[VescHwInterface]: Invalid end-of-frame character
[ros2_control_node-1] [ERROR] [1693755709.163192823] 
[VescHwInterface]: Out-of-sync with VESC, discarding 8 bytes.
[ros2_control_node-1] [ERROR] [1693755709.203127802] 
[VescHwInterface]: Invalid end-of-frame character
[ros2_control_node-1] [ERROR] [1693755709.203179020] 
[VescHwInterface]: Out-of-sync with VESC, unknown data leading 
valid frame. Discarding 1 bytes

I tried using the controllers (joint_state_broadcaster and velocity_controller), which worked with a single VESC, launching it with two VESCs with two ros2_control tags including two separate actuator-type hardware. I also tried to change the velocity controller to diff_drive_controller and tried the same thing with two VESCs. But it fails every time. I am attaching the screenshot of my configuration and xacro file. Any suggestion will be really helpful.

This is the config file for the controller manager and xacro file for the resource manager that works with one vesc.

controller_manager:
  ros__parameters:
    update_rate: 100 # Hz
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    joint_velocity_controller:
      type: velocity_controllers/JointGroupVelocityController

# Publish all joint states -----------------------------------
joint_state_broadcaster:
  ros__parameters:
    publish_rate: 100

joint_velocity_controller:
  ros__parameters:
    joints:
      - left_wheel_joint
    publish_rate: 100

The xacro that works with one vesc

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <ros2_control name="vesc" type="actuator">
    <hardware>
      <plugin>vesc_hw_interface/VescHwInterface</plugin>
      <param name="port">/dev/ttyACM1</param>
      <param name="gear_ratio">1</param>
      <param name="command_mode">velocity_duty</param>
      <param name="joint_type">continuous</param>
      <param name="num_hall_sensors">3</param>
      <param name="num_rotor_poles">20</param>
      <param name="motor/Kp">0.1</param>
      <param name="motor/Ki">0.01</param>
      <param name="motor/Kd">0.02</param>
      <param name="motor/i_clamp">1.0</param>
      <param name="motor/duty_limiter">1.0</param>
      <param name="motor/antiwindup">true</param>
      <param name="motor/control_rate">100.0</param>
      <param name="motor/enable_smooth_diff">true</param>
      <param name="motor/smooth_diff/max_sample_sec">0.2</param>
      <param name="motor/smooth_diff/max_smooth_step">10</param>

    </hardware>
    <joint name="right_wheel_joint">
      <!-- <command_interface name="position">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface> -->
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <!-- <command_interface name="effort">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface> -->
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <!-- <state_interface name="effort"/> -->
    </joint>

  </ros2_control>
</robot>

And these are what I tried.

Config file for controller manager with diff_drive_controller and joint_state_broadcaster

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diffbot_base_controller:
      type: diff_drive_controller/DiffDriveController

diffbot_base_controller:
  ros__parameters:
    left_wheel_names: ["left_wheel_joint"]
    right_wheel_names: ["right_wheel_joint"]

    wheel_separation: 0.221
    #wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
    wheel_radius: 0.045

    wheel_separation_multiplier: 1.0
    left_wheel_radius_multiplier: 1.0
    right_wheel_radius_multiplier: 1.0

    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link
    pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]

    open_loop: false
    enable_odom_tf: true

    cmd_vel_timeout: 0.5
    #publish_limited_velocity: true
    use_stamped_vel: false
    #velocity_rolling_window_size: 10

    # Velocity and acceleration limits
    # Whenever a min_* is unspecified, default to -max_*
    linear.x.has_velocity_limits: true
    linear.x.has_acceleration_limits: true
    linear.x.has_jerk_limits: false
    linear.x.max_velocity: 1.0
    linear.x.min_velocity: -1.0
    linear.x.max_acceleration: 1.0
    linear.x.max_jerk: 0.0
    linear.x.min_jerk: 0.0

    angular.z.has_velocity_limits: true
    angular.z.has_acceleration_limits: true
    angular.z.has_jerk_limits: false
    angular.z.max_velocity: 1.0
    angular.z.min_velocity: -1.0
    angular.z.max_acceleration: 1.0
    angular.z.min_acceleration: -1.0
    angular.z.max_jerk: 0.0
    angular.z.min_jerk: 0.0

joint_state_broadcaster:
  ros__parameters:
    publish_rate: 100

Xacro file for the resource manager with two vescs as actuators.

<?xml version="1.0" encoding="UTF-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <ros2_control name="VESCLeft" type="actuator">
    <hardware>
      <plugin>vesc_hw_interface/VescHwInterface</plugin>
      <param name="port">/dev/ttyACM0</param>
      <param name="gear_ratio">1</param>
      <param name="command_mode">velocity</param>
      <param name="joint_type">continuous</param>
      <param name="num_hall_sensors">3</param>
      <param name="num_rotor_poles">20</param>
      <param name="motor/Kp">0.1</param>
      <param name="motor/Ki">0.01</param>
      <param name="motor/Kd">0.02</param>
      <param name="motor/i_clamp">1.0</param>
      <param name="motor/duty_limiter">1.0</param>
      <param name="motor/antiwindup">true</param>
      <param name="motor/control_rate">50</param>
      <param name="motor/enable_smooth_diff">true</param>
      <param name="motor/smooth_diff/max_sample_sec">0.2</param>
      <param name="motor/smooth_diff/max_smooth_step">10</param>

    </hardware>
    <joint name="left_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-5</param>
        <param name="max">5</param>
      </command_interface>

      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>

  </ros2_control>

  <ros2_control name="VESCRight" type="actuator">
    <hardware>
      <plugin>vesc_hw_interface/VescHwInterface</plugin>
      <param name="port">/dev/ttyACM1</param>
      <param name="gear_ratio">1</param>
      <param name="command_mode">velocity</param>
      <param name="joint_type">continuous</param>
      <param name="num_hall_sensors">3</param>
      <param name="num_rotor_poles">20</param>
      <param name="motor/Kp">0.1</param>
      <param name="motor/Ki">0.01</param>
      <param name="motor/Kd">0.02</param>
      <param name="motor/i_clamp">1.0</param>
      <param name="motor/duty_limiter">1.0</param>
      <param name="motor/antiwindup">true</param>
      <param name="motor/control_rate">50</param>
      <param name="motor/enable_smooth_diff">true</param>
      <param name="motor/smooth_diff/max_sample_sec">0.2</param>
      <param name="motor/smooth_diff/max_smooth_step">10</param>

    </hardware>

    <joint name="right_wheel_joint">
      <command_interface name="velocity">
        <param name="min">-5</param>
        <param name="max">5</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>

    </joint>

  </ros2_control>
jsupratman13 commented 10 months ago

Invalid checksum [ros2_control_node-1] [ERROR] [1693755709.123338959] [VescHwInterface]: Out-of-sync with VESC, discarding 49 bytes. [ros2_control_node-1] [ERROR] [1693755709.128419540] [VescHwInterface]: Invalid end-of-frame character [ros2_control_node-1] [ERROR] [1693755709.128438065] [VescHwInterface]: Invalid payload length [ros2_control_node-1] [ERROR] [1693755709.128448735] [VescHwInterface]: Out-of-sync with VESC, discarding 21 bytes. [ros2_control_node-1] [ERROR] [1693755709.163161253] [VescHwInterface]: Invalid end-of-frame character [ros2_control_node-1] [ERROR] [1693755709.163192823] [VescHwInterface]: Out-of-sync with VESC, discarding 8 bytes. [ros2_control_node-1] [ERROR] [1693755709.203127802] [VescHwInterface]: Invalid end-of-frame character [ros2_control_node-1] [ERROR] [1693755709.203179020] [VescHwInterface]: Out-of-sync with VESC, unknown data leading valid frame. Discarding 1 bytes

Does this error log appear once or does it stream continuously?

Tacha-S commented 10 months ago

If VESC works when there's only one but consistently shows such a log when there are two or more, I have seen similar behavior before. I don't have time for an in-depth investigation, but first, try adding VESC to the dialout group in udev rules and then test it again.

Persius48 commented 10 months ago

Invalid checksum [ros2_control_node-1] [ERROR] [1693755709.123338959] [VescHwInterface]: Out-of-sync with VESC, discarding 49 bytes. [ros2_control_node-1] [ERROR] [1693755709.128419540] [VescHwInterface]: Invalid end-of-frame character [ros2_control_node-1] [ERROR] [1693755709.128438065] [VescHwInterface]: Invalid payload length [ros2_control_node-1] [ERROR] [1693755709.128448735] [VescHwInterface]: Out-of-sync with VESC, discarding 21 bytes. [ros2_control_node-1] [ERROR] [1693755709.163161253] [VescHwInterface]: Invalid end-of-frame character [ros2_control_node-1] [ERROR] [1693755709.163192823] [VescHwInterface]: Out-of-sync with VESC, discarding 8 bytes. [ros2_control_node-1] [ERROR] [1693755709.203127802] [VescHwInterface]: Invalid end-of-frame character [ros2_control_node-1] [ERROR] [1693755709.203179020] [VescHwInterface]: Out-of-sync with VESC, unknown data leading valid frame. Discarding 1 bytes

Does this error log appear once or does it stream continuously?

Yes, the error message is continuous.

Persius48 commented 10 months ago

If VESC works when there's only one but consistently shows such a log when there are two or more, I have seen similar behavior before. I don't have time for an in-depth investigation, but first, try adding VESC to the dialout group in udev rules and then test it again.

The VESCs were already in the dialoutgroup in udev and I added the current user in the dialoutgroup. The problems are persistent. One VESC connects, but two do not.

erknl commented 3 months ago

Is the problem solved?

Tacha-S commented 3 months ago

We also tested two VESCs as a differential drive and did not observe similar issues. Regarding the advice to add the user to the dialout group, there was only a comment that it had been added, without any logs on how the udev rules were set. This is insufficient information for us to replicate the issue, so please provide more detailed information, logs, and settings that would allow us to reproduce the problem.