Fix joint_velocity_controller.

diff --git a/twil_bringup/CMakeLists.txt b/twil_bringup/CMakeLists.txt
index 187e527..454ea1f 100644 (file)
--- a/twil_bringup/CMakeLists.txt
+++ b/twil_bringup/CMakeLists.txt
@@ -8,7 +8,7 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 
-install(PROGRAMS scripts/test_openloop.sh
+install(PROGRAMS scripts/test_openloop.sh scripts/set_wheel_velocity.sh
        DESTINATION lib/${PROJECT_NAME}
 )
 

diff --git a/twil_bringup/config/joint_velocity_controller.yaml b/twil_bringup/config/joint_velocity_controller.yaml
index c2fa9c8..69514e1 100644 (file)
--- a/twil_bringup/config/joint_velocity_controller.yaml
+++ b/twil_bringup/config/joint_velocity_controller.yaml
@@ -8,16 +8,14 @@ joint_state_broadcaster:
                         - front_caster_base_joint
                         - tower_joint
 
-left_wheel_joint_velocity_controller:
+joint_velocity_controller:
         ros__parameters:
-                type: effort_controllers/JointVelocityController
-                joint: left_wheel_joint
-                pid: {p: 0.0, i: 0.0, d: 0.0}
-                interface_name: effort
-
-right_wheel_joint_velocity_controller:
-        ros__parameters:
-                type: effort_controllers/JointVelocityController
-                joint: right_wheel_joint
-                pid: {p: 0.0, i: 0.0, d: 0.0}
-                interface_name: effort
+                type: pid_controller/PidController
+                dof_names:
+                        - right_wheel_joint
+                        - left_wheel_joint
+                command_interface: effort
+                reference_and_state_interfaces: ["velocity"]
+                gains:
+                        right_wheel_joint: {p: 2.6, i: 0.0005, d: 0.0, i_clamp_max: 100.0, i_clamp_min: -100.0, antiwindup: false}
+                        left_wheel_joint: {p: 2.6, i: 0.0005, d: 0.0, i_clamp_max: 100.0, i_clamp_min: -100.0, antiwindup: false}

diff --git a/twil_bringup/launch/joint_velocity_controller.launch.xml b/twil_bringup/launch/joint_velocity_controller.launch.xml
index 163e91a..73878ff 100644 (file)
--- a/twil_bringup/launch/joint_velocity_controller.launch.xml
+++ b/twil_bringup/launch/joint_velocity_controller.launch.xml
@@ -1,7 +1,7 @@
 <!--******************************************************************************
                               Twil Bringup
                    Joint Velocity Controller Launch File
-          Copyright (C) 2014, 2022 Walter Fetter Lages <w.fetter@ieee.org>
+          Copyright (C) 2014, 2025 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -22,11 +22,8 @@
 <launch>
        <arg name="config" default="$(find-pkg-share twil_bringup)/config/joint_velocity_controller.yaml"/>
 
-       <node name="left_wheel_controller_spawner" pkg="controller_manager" exec="spawner"
-               args="-p $(var config) left_wheel_velocity_controller"/>
-
-       <node name="right_wheel_controller_spawner" pkg="controller_manager" exec="spawner"
-               args="-p $(var config) right_wheel_velocity_controller"/>
+       <node name="pid_controller_spawner" pkg="controller_manager" exec="spawner"
+               args="-p $(var config) joint_velocity_controller"/>
                
        <node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
                args="-p $(var config) joint_state_broadcaster"/>

diff --git a/twil_bringup/scripts/set_wheel_velocity.sh b/twil_bringup/scripts/set_wheel_velocity.sh
new file mode 100755 (executable)
index 0000000..b90acbe
--- /dev/null
+++ b/twil_bringup/scripts/set_wheel_velocity.sh
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+if [ "$#" -ne 2 ]; then
+        echo "Usage: $0 <rght_wheel_velocity left_wheel_velocity>"
+        exit -1;
+fi;
+
+ros2 topic pub /joint_velocity_controller/reference control_msgs/msg/MultiDOFCommand "{dof_names: ["right_wheel_joint", "left_wheeljoint"], values: [$1, $2]}" -1

diff --git a/twil_bringup/scripts/test_openloop.sh b/twil_bringup/scripts/test_openloop.sh
index c83f241..60e6df8 100755 (executable)
--- a/twil_bringup/scripts/test_openloop.sh
+++ b/twil_bringup/scripts/test_openloop.sh
@@ -1,6 +1,6 @@
 #!/bin/bash
-ros2 topic pub -1 /left_wheel_joint_effort_controller/commands std_msgs/msg/Float64MultiArray '{data: [0.5]}'
-ros2 topic pub -1 /right_wheel_joint_effort_controller/commands std_msgs/msg/Float64MultiArray '{data: [0.5]}'
+ros2 topic pub -1 /left_wheel_joint_effort_controller/commands std_msgs/msg/Float64MultiArray '{data: [2.0]}'
+ros2 topic pub -1 /right_wheel_joint_effort_controller/commands std_msgs/msg/Float64MultiArray '{data: [2.0]}'
 sleep 3
 ros2 topic pub -1 /left_wheel_joint_effort_controller/commands std_msgs/msg/Float64MultiArray '{data: [0.0]}'
 ros2 topic pub -1 /right_wheel_joint_effort_controller/commands std_msgs/msg/Float64MultiArray '{data: [0.0]}'