From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Thu, 8 May 2025 07:06:18 +0000 (-0300)
Subject: Fix joint_velocity_controller.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=3d50b6d2bceae542bec35c432afa9b7b2667be57;p=twil.git

Fix joint_velocity_controller.
---

diff --git a/twil_bringup/CMakeLists.txt b/twil_bringup/CMakeLists.txt
index 187e527..454ea1f 100644
--- 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
--- 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
--- 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
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
--- 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]}'