From 17d19e393faf7d9d47e46f9c23f8bd4be50d0acb Mon Sep 17 00:00:00 2001
From: pimpest <82343504+pimpest@users.noreply.github.com>
Date: Mon, 9 Feb 2026 15:08:12 +0100
Subject: [PATCH] Added the ability to switch between cmd_vel control and
induvidual motor control
---
scripts/test_wheel.sh | 13 +++++-
toid_control/launch/toid.launch.py | 43 +++++++++++++++-----
toid_control/package.xml | 1 +
toid_control/params/toid_general_params.yaml | 9 ++++
toid_odometry/CMakeLists.txt | 1 +
toid_odometry/package.xml | 1 +
toid_odometry/src/odometry.cpp | 32 +++++++++++++++
7 files changed, 87 insertions(+), 13 deletions(-)
diff --git a/scripts/test_wheel.sh b/scripts/test_wheel.sh
index 6606851..6e0efcc 100755
--- a/scripts/test_wheel.sh
+++ b/scripts/test_wheel.sh
@@ -17,8 +17,6 @@ fi
source install/setup.bash
-trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
-
case $TARGET in
"forward")
ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped "
@@ -75,8 +73,19 @@ case $TARGET in
"zero")
ros2 service call /zero std_srvs/srv/Empty
;;
+ "vel")
+ ros2 topic pub --rate 10 /velocity_controller/commands std_msgs/msg/Float64MultiArray "
+ data: [$2, $3]"
+ ;;
+ "switch")
+ controllers=$(ros2 control list_controllers | grep -E "velocity|diffdrive")
+ to_deactivate=$(echo "$controllers" | awk '$3 == "active" { print $1 }')
+ to_activate=$(echo "$controllers" | awk '$3 == "inactive" { print $1 }')
+ ros2 control switch_controllers --deactivate $to_deactivate --activate $to_activate --best-effort
+ ;;
*)
echo "Target not defined"
exit 1
;;
esac
+exit 0
diff --git a/toid_control/launch/toid.launch.py b/toid_control/launch/toid.launch.py
index 90060f8..1aa3cb9 100644
--- a/toid_control/launch/toid.launch.py
+++ b/toid_control/launch/toid.launch.py
@@ -35,18 +35,11 @@ def generate_launch_description():
description="Whether to use mock controller"
)
- # odom_broadcast = Node(
- # package='tf2_ros',
- # executable='static_transform_publisher',
- # name='map_to_odom_broadcaster',
- # arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
- # condition=IfCondition(LaunchConfiguration('visualize'))
- # )
-
odom_broadcast = Node(
package='toid_odometry',
executable='toid_odometry',
name='map_to_odom_broadcaster',
+ emulate_tty=True,
parameters=[{'mock_odom': use_mock}],
condition=IfCondition(LaunchConfiguration('visualize'))
)
@@ -57,6 +50,7 @@ def generate_launch_description():
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
+ emulate_tty=True,
parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
)
@@ -64,20 +58,42 @@ def generate_launch_description():
package='controller_manager',
executable='ros2_control_node',
output='screen',
- parameters=[params]
+ emulate_tty=True,
+ parameters=[params],
+ arguments=['--ros-args', '--log-level', 'warn']
)
joint_state_broadcaster = Node(
package='controller_manager',
executable='spawner',
output='screen',
- arguments=["joint_state_broadcaster"]
+ emulate_tty=True,
+ arguments=[
+ "joint_state_broadcaster",
+ '--ros-args', '--log-level', 'warn'
+ ]
)
+ velocity_controller = Node(
+ package='controller_manager',
+ executable='spawner',
+ output='screen',
+ emulate_tty=True,
+ arguments=[
+ "velocity_controller",
+ "--inactive",
+ "-p",
+ params,
+ '--ros-args', '--log-level', 'warn'
+ ],
+ )
+
+
diffbot_base_controller = Node(
package='controller_manager',
executable='spawner',
output='both',
+ emulate_tty=True,
arguments=[
"diffdrive_controller",
"-p",
@@ -96,6 +112,7 @@ def generate_launch_description():
"--param enable_odom_tf:=true",
"--param enable_odom_tf:=false"
),
+ '--ros-args', '--log-level', 'warn'
]
)
@@ -104,7 +121,10 @@ def generate_launch_description():
executable='rviz2',
name='rviz2',
output='screen',
- arguments=['-d', default_rviz_config_path],
+ emulate_tty=True,
+ arguments=['-d', default_rviz_config_path,
+ '--ros-args', '--log-level', 'warn'
+ ],
condition=IfCondition(visualize)
)
@@ -116,5 +136,6 @@ def generate_launch_description():
controller_manager,
joint_state_broadcaster,
diffbot_base_controller,
+ velocity_controller,
rviz_node
])
diff --git a/toid_control/package.xml b/toid_control/package.xml
index d96c227..1bf04bc 100644
--- a/toid_control/package.xml
+++ b/toid_control/package.xml
@@ -21,6 +21,7 @@
controller_manager
diff_drive_controller
+ forward_command_controller
robot_state_publisher
joint_state_broadcaster
diff --git a/toid_control/params/toid_general_params.yaml b/toid_control/params/toid_general_params.yaml
index 6a6cb64..bcd387a 100644
--- a/toid_control/params/toid_general_params.yaml
+++ b/toid_control/params/toid_general_params.yaml
@@ -6,6 +6,15 @@ controller_manager:
type: diff_drive_controller/DiffDriveController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
+ velocity_controller:
+ type: forward_command_controller/ForwardCommandController
+
+velocity_controller:
+ ros__parameters:
+ joints:
+ - drivewhl_l_joint
+ - drivewhl_r_joint
+ interface_name: velocity
diffdrive_controller:
ros__parameters:
diff --git a/toid_odometry/CMakeLists.txt b/toid_odometry/CMakeLists.txt
index 9ee98c3..cdb0706 100644
--- a/toid_odometry/CMakeLists.txt
+++ b/toid_odometry/CMakeLists.txt
@@ -10,6 +10,7 @@ find_package(ament_cmake REQUIRED)
set(PACKAGE_DEPS
rclcpp
Boost
+ nav_msgs
std_srvs
tf2
tf2_ros
diff --git a/toid_odometry/package.xml b/toid_odometry/package.xml
index 41b160c..f531967 100644
--- a/toid_odometry/package.xml
+++ b/toid_odometry/package.xml
@@ -11,6 +11,7 @@
rclcpp
boost
+ nav_msgs
std_srvs
tf2
tf2_ros
diff --git a/toid_odometry/src/odometry.cpp b/toid_odometry/src/odometry.cpp
index 91b29d0..b99d7d8 100644
--- a/toid_odometry/src/odometry.cpp
+++ b/toid_odometry/src/odometry.cpp
@@ -23,9 +23,20 @@
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/convert.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
namespace asio = boost::asio;
+const std::array COVARIENCE_MATRIX{
+ 0.0001, .0, 0, 0, 0, 0,
+ 0, 0.0001, 0, 0, 0, 0,
+ 0, .0, 0.0001, 0, 0, 0,
+ 0, .0, 0, 0.0001, 0, 0,
+ 0, .0, 0, 0, 0.0001, 0,
+ 0, .0, 0, 0, 0, 0.0001,
+};
+
class ToidOdomNode : public rclcpp::Node {
using SendDoubleSrv = toid_msgs::srv::SendDouble;
using ZeroSrv = std_srvs::srv::Empty;
@@ -78,6 +89,9 @@ public:
zero_service_ = create_service (
"/zero", std::bind(&ToidOdomNode::zero, this, _1));
+
+ odometry_publisher_ = this->create_publisher(
+ "/odom", 1);
}
}
@@ -179,13 +193,30 @@ private:
void send_transform(tRespState &state) {
geometry_msgs::msg::TransformStamped t;
+ nav_msgs::msg::Odometry o;
t.header.stamp = this->get_clock()->now();
t.header.frame_id = this->odometry_frame_id_;
t.child_frame_id = this->base_frame_id_;
+ o.header = t.header;
+ o.child_frame_id = t.child_frame_id;
+
t.transform = make_transform(state.x, state.y, state.theta);
+ const auto& [x,y,z] = t.transform.translation;
+ o.pose.pose.position.x = x;
+ o.pose.pose.position.x = y;
+ o.pose.pose.position.x = z;
+ o.pose.pose.orientation = t.transform.rotation;
+ o.pose.covariance = COVARIENCE_MATRIX;
+
+ o.twist.twist.linear.x = state.vl;
+ o.twist.twist.angular.z = state.vr;
+ o.twist.covariance = COVARIENCE_MATRIX;
+
+ odometry_publisher_->publish(o);
+
tf_broadcaster_->sendTransform(t);
}
@@ -237,6 +268,7 @@ private:
rclcpp::Service::SharedPtr zero_service_;
rclcpp::Subscription::SharedPtr startpose_listener_;
+ rclcpp::Publisher::SharedPtr odometry_publisher_;
};
int main(const int argc,const char** argv) {