Added effort example

Signed-off-by: ahcorde <ahcorde@gmail.com>

README.md

@@ -209,6 +209,7 @@ You can run some of the configuration running the following commands:
 ros2 launch gazebo_ros2_control_demos cart_example_position_pid.launch.py
 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
 ```
 
 Send example commands:
@@ -218,6 +219,7 @@ When the Gazebo world is launched you can run some of the following commads to m
 ```bash
 ros2 run gazebo_ros2_control_demos example_position
 ros2 run gazebo_ros2_control_demos example_velocity
+ros2 run gazebo_ros2_control_demos example_effort
 ```
 
 To get or modify the values of the PID controller you can run the following commands:

gazebo_ros2_control_demos/CMakeLists.txt

@@ -40,6 +40,12 @@ ament_target_dependencies(example_velocity
   std_msgs
 )
 
+add_executable(example_effort examples/example_effort.cpp)
+ament_target_dependencies(example_effort
+  rclcpp
+  std_msgs
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_gtest REQUIRED)
   find_package(ament_lint_auto REQUIRED)
@@ -48,7 +54,7 @@ if(BUILD_TESTING)
 endif()
 
 ## Install
-install(TARGETS example_position example_velocity
+install(TARGETS example_position example_velocity example_effort
   DESTINATION lib/${PROJECT_NAME}
 )
 

gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml

@@ -0,0 +1,17 @@
+cart_pole_controller:
+  ros__parameters:
+    type: effort_controllers/JointGroupEffortController
+    joints:
+       - slider_to_cart
+    write_op_modes:
+       - slider_to_cart
+    state_publish_rate: 25
+    action_monitor_rate: 20
+    constraints:
+      stopped_velocity_tolerance: 0.05
+      goal_time: 5
+
+joint_state_controller:
+  ros__parameters:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50

gazebo_ros2_control_demos/examples/example_effort.cpp

@@ -0,0 +1,58 @@
+// Copyright 2020 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 <string>
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "std_msgs/msg/float64_multi_array.hpp"
+
+std::shared_ptr<rclcpp::Node> node;
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  node = std::make_shared<rclcpp::Node>("effort_test_node");
+
+  auto publisher = node->create_publisher<std_msgs::msg::Float64MultiArray>(
+    "/cart_pole_controller/commands", 10);
+
+  RCLCPP_INFO(node->get_logger(), "node created");
+
+  std_msgs::msg::Float64MultiArray commands;
+
+  using namespace std::chrono_literals;
+
+  commands.data.push_back(0);
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 100;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = -200;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+
+  commands.data[0] = 0;
+  publisher->publish(commands);
+  std::this_thread::sleep_for(1s);
+  rclcpp::shutdown();
+
+  return 0;
+}

gazebo_ros2_control_demos/launch/cart_example_effort.launch.py

@@ -0,0 +1,62 @@
+# Copyright 2020 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 IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_cart_effort.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'cartpole'],
+                        output='screen')
+
+    return LaunchDescription([
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])

gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf

@@ -0,0 +1,69 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from cartpole.xacro.urdf            | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="cartopole">
+  <link name="world"/>
+  <link name="slideBar">
+    <visual>
+      <geometry>
+        <box size="30 0.05 0.05"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="green">
+        <color rgba="0 0.8 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="100"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <link name="cart">
+    <visual>
+      <geometry>
+        <box size="0.5 0.5 0.2"/>
+      </geometry>
+      <origin xyz="0 0 0"/>
+      <material name="blue">
+        <color rgba="0 0 .8 1"/>
+      </material>
+    </visual>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+  <joint name="world_to_base" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 1"/>
+    <parent link="world"/>
+    <child link="slideBar"/>
+  </joint>
+  <joint name="slider_to_cart" type="prismatic">
+    <axis xyz="1 0 0"/>
+    <origin xyz="0.0 0.0 0.0"/>
+    <parent link="slideBar"/>
+    <child link="cart"/>
+    <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>
+    <dynamics damping="0.0" friction="0.0"/>
+  </joint>
+  <transmission name="slider_to_cart_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="slider_to_cart">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="slider_to_cart_motor">
+      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <gazebo>
+    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
+      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
+      <parameters>$(find gazebo_ros2_control_demos)/config/cartpole_controller_effort.yaml</parameters>
+      <control_period>0.01</control_period>
+      <e_stop_topic>/cart_pole/estop</e_stop_topic>
+    </plugin>
+  </gazebo>
+</robot>