Ask Your Question
1

Diff_drive_controller not subscribing to /cmd_vel

asked 2020-03-04 12:12:22 -0500

ndimick gravatar image

updated 2020-03-10 13:17:11 -0500

Hi, my team is working on a differential drive robot and trying to decide if the diff_drive_controller is a good fit for our project. We have been borrowing and adapting code from projects like My_ROS_mobile_robot, ros_control_boilerplate, and the hardware_interface tutorial.

Our problem right now is that when we launch all the nodes that we believe we need, there is no subscription to /cmd_vel as we would expect, given diff_drive_controller's documentation. I'll leave some snippets of our code below. Here is a link to our full repository, with relevant code to this problem located in the hw_interface and launch directories.

Here is our relevant code: diff_drive.launch:

<launch>
<!-- Hardware Interface -->
<node name='hardware_interface' type='hw_interface_node' pkg='hw_inter'/>
<!-- URDF -->
<arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find rover_4_core)/description/urdf/rover.urdf'" />
<param name="robot_description" command="$(arg urdf_file)" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<!-- Load controller config -->
<rosparam command="load" 
          file="$(find rover_4_core)/hw_interface/config/common.yaml"/>
<rosparam command="load" 
          file="$(find rover_4_core)/hw_interface/config/control.yaml"/>

<node name="controller_spawner" 
      pkg="controller_manager" 
      type="spawner" 
      output="screen" 
      ns="/"
      args="mobile_base_controller joint_state_controller leftWheel_effort_controller rightWheel_effort_controller"/>    
</launch>

control.yaml:

my_robot:
motor_commands:
    pwm_update_frequency: 50.0
    cmd_vel_topic:
      left:  "/cmd_vel"
      right: "/cmd_vel"
  # Publish all joint states -----------------------------------
  # Creates the /joint_states topic necessary in ROS
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

  # Effort Controllers ---------------------------------------
  leftWheel_effort_controller:
    type: effort_controllers/JointEffortController
    joint: lwheel_to_base
    pid: {p: 100.0, i: 0.1, d: 10.0}
    #pid: {p: 50.0, i: 0.1, d: 0.0}
  rightWheel_effort_controller:
    type: effort_controllers/JointEffortController
    joint: rwheel_to_base
    pid: {p: 100.0, i: 0.1, d: 10.0}
    #pid: {p: 50.0, i: 0.1, d: 0.0}

common.yaml:

mobile_base_controller:
  type        : "diff_drive_controller/DiffDriveController"
  left_wheel  : 'left_wheel'
  right_wheel : 'right_wheel'
  publish_rate: 50.0  # default: 50
  pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
  twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

  # Wheel separation and diameter. These are both optional.
  # diff_drive_controller will attempt to read either one or both from the
  # URDF if not specified as a parameter
  wheel_separation : 0.026 # meters 
  wheel_radius : 0.0045    # meters

  # Wheel separation and radius multipliers
  wheel_separation_multiplier: 1.0 # default: 1.0
  wheel_radius_multiplier    : 1.0 # default: 1.0

  # Velocity commands timeout [s], default 0.5
  cmd_vel_timeout: 0.25

  # Base frame_id
  base_frame_id: base_link #default: base_link
  # Velocity and acceleration limits
  # Whenever a min_* is unspecified, default to -max_*
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 1.15 #1.0  # m/s
      min_velocity           : -1.15 #-0.5 # m/s
      has_acceleration_limits: true
      max_acceleration       : 5.0 #0.8  # m/s^2
      min_acceleration       : -1.0 #-0.4 # m/s^2
      has_jerk_limits        : true
      max_jerk               : 5.0  # m/s^3
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 50.0 #1.7  # rad/s
      has_acceleration_limits: true
      max_acceleration       : 1.5  # rad/s^2
      has_jerk_limits        : true
      max_jerk               : 2.5  # rad/s^3

#Publish to TF directly or not
enable_odom_tf: true

#Name of frame to publish odometry in
odom_frame_id: odom

# Publish the velocity command to be executed. 
# It is to ...
(more)
edit retag flag offensive close merge delete

Comments

Your .launch file suggests you are attempting to start all of these controllers together: mobile_base_controller joint_state_controller leftWheel_effort_controller rightWheel_effort_controller.

That cannot work, as the effort controllers will try to claim the same resources as your mobile_base_controller.

Can you show the output on the console after you roslaunch [..] diff_drive.launch?

gvdhoorn gravatar image gvdhoorn  ( 2020-03-04 15:16:14 -0500 )edit

Hi, @gvdhoorn, thanks for your help! I've edited the original question to include the console output at the bottom of the question.

ndimick gravatar image ndimick  ( 2020-03-04 16:09:04 -0500 )edit

If that's all there is then I'm not sure everything is being started.

Can you try launching using

roslaunch --screen rover_4_core diff_drive.launch

Is this one machine btw? Because I see this in your output:

ROS_MASTER_URI=http://robot.dyn.brandeis.edu:11311

If this is multi-machine then please make sure DNS is working properly (in both ways, so forward and reverse lookup).

gvdhoorn gravatar image gvdhoorn  ( 2020-03-05 01:58:15 -0500 )edit

launching using

roslaunch --screen rover_4_core diff_drive.launch

produced the same console output as before. No new or additional lines.

Yes we are on a multi-machine setup, launching diff_drive.launch from a remote pc, running roscore on a raspberry pi attached to our hardware. We tested DNS lookups with dig and it is working properly both ways from both machines. Ros topics are also communicating between the two without issue.

ndimick gravatar image ndimick  ( 2020-03-05 11:27:15 -0500 )edit

@gvdhoorn I am working with @ndimick. We're wondering if you (or anyone else) has further insight into the problem above and/or whether you need some more information? Thanks

pitosalas gravatar image pitosalas  ( 2020-03-09 14:36:06 -0500 )edit

I find the console log you show suspicious. I'd expect a line mentioning something like "started controller .." there as well.

Try listing the controllers loaded and ascertaining their status.

Also: what is the output of rosnode info hardware_interface after you've started everything?

gvdhoorn gravatar image gvdhoorn  ( 2020-03-09 15:47:53 -0500 )edit

Should the controller start automatically after it is spawned or does it have to be manually started inside of a node?

ndimick gravatar image ndimick  ( 2020-03-09 20:11:30 -0500 )edit

Please answer my other questions as well.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-10 03:18:12 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2020-03-10 16:17:43 -0500

gvdhoorn gravatar image

updated 2020-03-10 16:22:56 -0500

This is the main(..) from the code you link in your question:

int main(int argc, char **argv)
{
    /*
    * Main loop of the hardware interface.
    */
    ros::init(argc, argv, "hw_interface");
    MyRobot robot;
    controller_manager::ControllerManager cm(&robot);
    ros::Time time_now;
    ros::Duration period_now;
    ros::Rate sleep_rate(10);


    while (true)
    {
        time_now = robot.get_time();
        period_now = robot.get_period(); 
        robot.read(period_now);
        cm.update(time_now, period_now);
        robot.write();
        sleep_rate.sleep();
    }
}

There is one thing suspiciously missing here, and that would be somewhere for ROS to actually process events. The fact that the controller_manager services do not produce any output is indicative of this.

Even a hardware_interface needs some time to process incoming and outgoing events, but you don't give it any.

You'll either want to call ros::spinOnce() somewhere in your while loop, or instantiate a ros::AsyncSpinner and start it. The latter would actually be recommended. Something like this should do the trick:

ros::AsyncSpinner spinner(2);
spinner.start();

A single thread is not enough, as there is a high potential for deadlocks.

Note that this also done in the two examples that you link: PickNikRobotics/ros_control_boilerplate (here) and eborghi10/my_ROS_mobile_robot (here).


Edit: just noticed you have this:

while (true)
{
  [..]
}

This is not a good idea in a ROS node -- which a hardware_interface essentially is.

You'll want to check for ros::ok() instead.

edit flag offensive delete link more

Comments

Thank you! adding the ros::AsyncSpinner did the trick. Again, thanks for your time, help and additional insights.

ndimick gravatar image ndimick  ( 2020-03-11 12:15:35 -0500 )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

3 followers

Stats

Asked: 2020-03-04 12:12:22 -0500

Seen: 89 times

Last updated: Mar 10