Skip to content

Commit

Permalink
Replaced diff drive controller to gazebo plugin and remap cmd_vel topic
Browse files Browse the repository at this point in the history
Signed-off-by: Darby Lim <thlim@robotis.com>
  • Loading branch information
Darby Lim committed Sep 28, 2022
1 parent 5324a27 commit 62c83db
Show file tree
Hide file tree
Showing 3 changed files with 24 additions and 62 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -6,9 +6,6 @@ controller_manager:
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster

diff_drive_controller:
type: diff_drive_controller/DiffDriveController

imu_broadcaster:
type: imu_sensor_broadcaster/IMUSensorBroadcaster

Expand All @@ -18,65 +15,6 @@ controller_manager:
gripper_controller:
type: position_controllers/GripperActionController

diff_drive_controller:
ros__parameters:
use_sim_time: True

left_wheel_names: ["wheel_left_joint"]
right_wheel_names: ["wheel_right_joint"]

wheel_separation: 0.287
#wheels_per_side: 1 # actually 2, but both are controlled by 1 signal
wheel_radius: 0.033

wheel_separation_multiplier: 1.0
left_wheel_radius_multiplier: 1.0
right_wheel_radius_multiplier: 1.0

publish_rate: 100.0
odom_frame_id: odom
base_frame_id: base_footprint
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: true
enable_odom_tf: true

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

# Preserve turning radius when limiting speed/acceleration/jerk
preserve_turning_radius: true

# Publish limited velocity
publish_cmd: true

# Publish wheel data
publish_wheel_data: true

# 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: 0.26
linear.x.min_velocity: -0.26
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.82
angular.z.min_velocity: -1.82
angular.z.max_acceleration: 4.0
angular.z.min_acceleration: -4.0
angular.z.max_jerk: 0.0
angular.z.min_jerk: 0.0

imu_broadcaster:
ros__parameters:
use_sim_time: True
Expand Down
2 changes: 2 additions & 0 deletions turtlebot3_manipulation_bringup/launch/base.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -125,6 +125,7 @@ def generate_launch_description():
{'robot_description': urdf_file},
controller_manager_config
],
remappings=[('~/cmd_vel_unstamped', 'cmd_vel')],
output={
'stdout': 'screen',
'stderr': 'screen',
Expand All @@ -144,6 +145,7 @@ def generate_launch_description():
executable='spawner.py',
arguments=['diff_drive_controller'],
output='screen',
condition=UnlessCondition(use_sim)
),

Node(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,28 @@ https://github.com/ROBOTIS-GIT/turtlebot3/blob/kinetic-devel/turtlebot3_descript
</sensor>
</gazebo>

<gazebo>
<plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
<ros>
<namespace></namespace>
<remapping>cmd_vel:=cmd_vel</remapping>
<remapping>odom:=odom</remapping>
</ros>

<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>
<wheel_separation>0.287</wheel_separation>
<wheel_diameter>0.066</wheel_diameter>
<max_wheel_torque>20</max_wheel_torque>
<max_wheel_acceleration>1.0</max_wheel_acceleration>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>false</publish_wheel_tf>
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_footprint</robot_base_frame>
</plugin>
</gazebo>

<gazebo reference="${prefix}base_scan">
<material>Gazebo/FlatBlack</material>
<sensor type="ray" name="lds_lfcd_sensor">
Expand Down

0 comments on commit 62c83db

Please sign in to comment.