Merge pull request #119 from azazdeaz/humble

ahcorde · web-flow · commit b64969b15a0e · 2023-02-13T15:35:42.000+01:00
Add tricycle example to the `humble` branch
diff --git a/README.md b/README.md
@@ -219,6 +219,7 @@ ros2 launch ign_ros2_control_demos cart_example_position.launch.py
 ros2 launch ign_ros2_control_demos cart_example_velocity.launch.py
 ros2 launch ign_ros2_control_demos cart_example_effort.launch.py
 ros2 launch ign_ros2_control_demos diff_drive_example.launch.py
+ros2 launch ign_ros2_control_demos tricycle_drive_example.launch.py
 ```
 
 Send example commands:
@@ -230,4 +231,5 @@ ros2 run ign_ros2_control_demos example_position
 ros2 run ign_ros2_control_demos example_velocity
 ros2 run ign_ros2_control_demos example_effort
 ros2 run ign_ros2_control_demos example_diff_drive
+ros2 run ign_ros2_control_demos example_tricycle_drive
 ```
diff --git a/ign_ros2_control_demos/CMakeLists.txt b/ign_ros2_control_demos/CMakeLists.txt
@@ -53,6 +53,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)
@@ -67,6 +73,7 @@ install(
     example_velocity
     example_effort
     example_diff_drive
+    example_tricycle_drive
   DESTINATION
     lib/${PROJECT_NAME}
 )
diff --git a/ign_ros2_control_demos/config/tricycle_drive_controller.yaml b/ign_ros2_control_demos/config/tricycle_drive_controller.yaml
@@ -0,0 +1,57 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50 # Hz
+
+    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.
diff --git a/ign_ros2_control_demos/examples/example_tricycle_drive.cpp b/ign_ros2_control_demos/examples/example_tricycle_drive.cpp
@@ -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;
+}
diff --git a/ign_ros2_control_demos/launch/tricycle_drive_example.launch.py b/ign_ros2_control_demos/launch/tricycle_drive_example.launch.py
@@ -0,0 +1,102 @@
+# 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
+from launch.actions import RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    # Launch Arguments
+    use_sim_time = LaunchConfiguration('use_sim_time', default=True)
+
+    ignition_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('ign_ros2_control_demos'))
+
+    xacro_file = os.path.join(ignition_ros2_control_demos_path,
+                              'urdf',
+                              'test_tricycle_drive.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    print(params)
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params],
+    )
+
+    ignition_spawn_entity = Node(
+        package='ros_gz_sim',
+        executable='create',
+        output='screen',
+        arguments=['-string', doc.toxml(),
+                   '-name', 'cartpole',
+                   '-allow_renaming', 'true'],
+    )
+
+    load_joint_state_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster'],
+        output='screen'
+    )
+
+    load_joint_trajectory_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'tricycle_controller'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        # Launch gazebo environment
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [os.path.join(get_package_share_directory('ros_gz_sim'),
+                              'launch', 'gz_sim.launch.py')]),
+            launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])]),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=ignition_spawn_entity,
+                on_exit=[load_joint_state_controller],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_controller,
+                on_exit=[load_joint_trajectory_controller],
+            )
+        ),
+        node_robot_state_publisher,
+        ignition_spawn_entity,
+        # Launch Arguments
+        DeclareLaunchArgument(
+            'use_sim_time',
+            default_value=use_sim_time,
+            description='If true, use simulated clock'),
+    ])
diff --git a/ign_ros2_control_demos/package.xml b/ign_ros2_control_demos/package.xml
@@ -40,6 +40,7 @@
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>velocity_controllers</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>
+  <exec_depend>tricycle_controller</exec_depend>
   <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf
