Command format passed to hardware_interface by DiffDriveController [closed]
Under my controller_manager
I have a DiffDriveController
and a hardware_interface
with two Joints (wheels). How do I know what type of command is sent by the DiffDriveController
to the cmd
arguments of the JointHandle
s in my hardware_interface
? Is it linear velocity, degrees per second, radians per second, or RPM? Can the command type be adjusted somewhere? Is this cmd
argument the same as the publish_cmd
published by the DiffDriveController
?
Snippet from the constructor of my hardware_interface
:
hardware_interface::JointStateHandle left_joint_state_handle(LEFT_JOINT_NAME,
&pos[LEFT_JOINT_IDX],
&vel[LEFT_JOINT_IDX],
&eff[LEFT_JOINT_IDX]);
jnt_state_interface_.registerHandle(left_joint_state_handle);
hardware_interface::JointStateHandle right_joint_state_handle(RIGHT_JOINT_NAME,
&pos[RIGHT_JOINT_IDX],
&vel[RIGHT_JOINT_IDX],
&eff[RIGHT_JOINT_IDX]);
jnt_state_interface_.registerHandle(right_joint_state_handle);
registerInterface(&jnt_state_interface_);
// LEFT_JOINT_NAME: "left_wheel_joint"; RIGHT_JOINT_NAME: "right_wheel_joint"
hardware_interface::JointHandle left_joint_vel_handle(jnt_state_interface_.getHandle(LEFT_JOINT_NAME),
&cmd[LEFT_JOINT_IDX]);
vel_jnt_interface_.registerHandle(left_joint_vel_handle);
hardware_interface::JointHandle right_joint_vel_handle(jnt_state_interface_.getHandle(RIGHT_JOINT_NAME),
&cmd[RIGHT_JOINT_IDX]);
vel_jnt_interface_.registerHandle(right_joint_vel_handle);
registerInterface(&vel_jnt_interface_);
DiffDriveController setup:
base_controller:
type : "diff_drive_controller/DiffDriveController"
left_wheel : 'left_wheel_joint' # joint 0
right_wheel : 'right_wheel_joint' # joint 1
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] # TODO adjust
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
wheel_separation : 0.18 # TODO measure in solidworks
wheel_radius : 0.03
linear:
x:
has_velocity_limits: true
max_velocity: 0.5
angular:
z:
has_velocity_limits: true
max_velocity: 2
enable_odom_tf: true
odom_frame_id: odom
publish_cmd: true
This seems like a duplicate of #q242766.
And a related questions: #q307520 and #q321078.
Finally: everything in ROS is assumed to be using SI (derived) units. So distances: metres. Angles: radians. Velocities: m/s or rad/s. Etc. Etc. Same for
diff_drive_controller
.See also REP-103.