Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add tricycle controller demo #145

Merged
merged 12 commits into from
Aug 15, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -223,7 +223,8 @@ You can run some of the configuration running the following commands:
ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py
ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py
ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py
ros2 launch gazebo_ros2_control_demos diff_drive_example.launch.py
ros2 launch gazebo_ros2_control_demos diff_drive.launch.py
ros2 launch gazebo_ros2_control_demos tricycle_drive.launch.py
```

Send example commands:
Expand All @@ -234,7 +235,8 @@ When the Gazebo world is launched you can run some of the following commads to m
ros2 run gazebo_ros2_control_demos example_position
ros2 run gazebo_ros2_control_demos example_velocity
ros2 run gazebo_ros2_control_demos example_effort
ros2 run ign_ros2_control_demos example_diff_drive
ros2 run gazebo_ros2_control_demos example_diff_drive
ros2 run gazebo_ros2_control_demos example_tricycle_drive
```

The following example shows parallel gripper with mimic joint:
Expand Down
7 changes: 7 additions & 0 deletions gazebo_ros2_control_demos/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,12 @@ ament_target_dependencies(example_diff_drive
geometry_msgs
)

add_executable(example_tricycle_drive examples/example_tricycle_drive.cpp)
ament_target_dependencies(example_tricycle_drive
rclcpp
geometry_msgs
)

if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_lint_auto REQUIRED)
Expand All @@ -73,6 +79,7 @@ install(
example_velocity
example_effort
example_diff_drive
example_tricycle_drive
example_gripper
DESTINATION
lib/${PROJECT_NAME}
Expand Down
229 changes: 229 additions & 0 deletions gazebo_ros2_control_demos/config/config.rviz
Original file line number Diff line number Diff line change
@@ -0,0 +1,229 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /TF1
- /RobotModel1
- /RobotModel1/Status1
- /RobotModel1/Description Topic1
Splitter Ratio: 0.5
Tree Height: 741
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz_common/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_link:
Value: true
chassis:
Value: true
left_wheel:
Value: true
odom:
Value: true
right_wheel:
Value: true
steering_link:
Value: true
wheel_front_link:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
odom:
base_link:
chassis:
left_wheel:
{}
right_wheel:
{}
steering_link:
wheel_front_link:
{}
Update Interval: 0
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: true
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
chassis:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
steering_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
wheel_front_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Mass Properties:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Covariance x: 0.25
Covariance y: 0.25
Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 9.402809143066406
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: -0.8695068955421448
Y: -0.05153140798211098
Z: -0.16355116665363312
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.2903985381126404
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.6204142570495605
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1032
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000002380000036efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000036e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000036efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000036e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005420000036e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1920
X: 1920
Y: 24
58 changes: 58 additions & 0 deletions gazebo_ros2_control_demos/config/tricycle_drive_controller.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,58 @@
controller_manager:
ros__parameters:
update_rate: 50 # Hz
use_sim_time: true

tricycle_controller:
type: tricycle_controller/TricycleController

joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster

joint_state_broadcaster:
ros__parameters:
extra_joints: ["right_wheel_joint", "left_wheel_joint"]

tricycle_controller:
ros__parameters:
# Model
traction_joint_name: traction_joint # Name of traction joint in URDF
steering_joint_name: steering_joint # Name of steering joint in URDF
wheel_radius: 0.3 # Radius of front wheel
wheelbase: 1.7 # Distance between center of back wheels and front wheel

# Odometry
odom_frame_id: odom
base_frame_id: base_link
publish_rate: 50.0 # publish rate of odom and tf
open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry
enable_odom_tf: true # If True, publishes odom<-base_link TF
odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf
pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source
twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source
velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom

# Rate Limiting
traction: # All values should be positive
# min_velocity: 0.0
# max_velocity: 1000.0
# min_acceleration: 0.0
max_acceleration: 5.0
# min_deceleration: 0.0
max_deceleration: 8.0
# min_jerk: 0.0
# max_jerk: 1000.0
steering:
min_position: -1.57
max_position: 1.57
# min_velocity: 0.0
max_velocity: 1.0
# min_acceleration: 0.0
# max_acceleration: 1000.0

# cmd_vel input
cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received
use_stamped_vel: false # Set to True if using TwistStamped.

# Debug
publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s.
53 changes: 53 additions & 0 deletions gazebo_ros2_control_demos/examples/example_tricycle_drive.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
// Copyright 2022 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <memory>

#include <rclcpp/rclcpp.hpp>

#include <geometry_msgs/msg/twist.hpp>

using namespace std::chrono_literals;

int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);

std::shared_ptr<rclcpp::Node> node =
std::make_shared<rclcpp::Node>("tricycle_drive_test_node");

auto publisher = node->create_publisher<geometry_msgs::msg::Twist>(
"/tricycle_controller/cmd_vel", 10);

RCLCPP_INFO(node->get_logger(), "node created");

geometry_msgs::msg::Twist command;

command.linear.x = 0.2;
command.linear.y = 0.0;
command.linear.z = 0.0;

command.angular.x = 0.0;
command.angular.y = 0.0;
command.angular.z = 0.1;

while (1) {
publisher->publish(command);
std::this_thread::sleep_for(50ms);
rclcpp::spin_some(node);
}
rclcpp::shutdown();

return 0;
}
2 changes: 1 addition & 1 deletion gazebo_ros2_control_demos/launch/diff_drive.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ def generate_launch_description():

spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'cartpole'],
'-entity', 'diffbot'],
output='screen')

load_joint_state_controller = ExecuteProcess(
Expand Down
Loading