diff --git a/.gitignore b/.gitignore index 4392efc..b17917a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ bin build lib - +test/.cache +test/__pycache__ diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 62c7067..c1da2f4 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,11 +4,11 @@ Velocity Smoother Forthcoming ----------- -* ... +* [tests] translational smoothing test added, `#7 `_ 0.13.0 (2020-01-20) ------------------- -* Refactored for ROS 2 and renamed to velocity_smoother, `#1 `_ +* [infra] refactored for ROS 2 and renamed to velocity_smoother, `#1 `_ 0.6.3 (2014-12-05) ------------------ diff --git a/README.md b/README.md new file mode 100644 index 0000000..5fd446f --- /dev/null +++ b/README.md @@ -0,0 +1,52 @@ +# Velocity Smoother + +[[About](#about)][[Parameters](#parameters)][[Topics](#topics)][[Usage](#usage)][[Feedback](#feedback)] + +## About + +ROS2 package for smoothing commanded velocities represented by a stream of +`geometry_msg/Twist` messages. It applies limits to linear and angular +components of both speed and acceleration. Feedback from either +the odometry / actual commanded velocity can be used to provide +a better result in some situations (read further below). + +![Profiles](test/translational_smoothing_profiles.png) + +## Parameters + +* **~accel_lim_v** (double, default: 0.8): linear acceleration limit +* **~accel_lim_w** (double, default: 5.4): angular acceleration limit +* **~speed_lim_v** (double, default: 0.3): linear velocity limit +* **~speed_lim_w** (double, default: 3.5): angular velocity limit +* **~decel_factor** (double, default: 1.0): multiplier for the acceleration limit when decelerating +* **~frequency** (double, default: 20.0): computed and published rate, adhered to regardless of the incoming message rate (interpolates when necessary) +* **~feedback** (int, default: 0): the type of feedback to use (0 - none, 1 - odometry, 2 - actual commanded velocities) + +## Topics + +Subscriptions + +* **~/input** (`geometry_msgs/Twist`): input velocity commands +* **~/feedback/odometry** (`nav_msgs/Odometry`): topic for odometry feedback, only required if the requested feedback has been set for odometry +* **~/feedback/cmd_vel** (`geometry_msgs/Twist`): topic for actual commanded velocity feedback, only required if the requested feedback has been set for actual commanded velocities + +Publications + +* **~smoothed** (`geometry_msgs/Twist`): smoothed output velocity commands respecting velocity and acceleration limits + +## Usage + +* All the parameters except frequency are dynamically reconfigurable. +* Linear and angular velocities are smoothed proportionally to the more restricted, so we guaranty a constant rotation radius. +* If the input topic gets inactive, and the last command is not a zero-velocity one, (maybe the controller crashed, or just forgot good manners...), we introduce a fake zero-velocity command after a sort timeout. + +Simply wire up the channels to their appropriate topics. The tests can be a useful starting point. + +## Feedback + +There are some reasons to use robot feedback. The two most frequently faced: + +* Multiple controllers compete for controlling the robot via a multiplexer. A controller that has been excluded by another with higher priority may suddenly issue commands significantly different to the +last commanded velocity when the multiplexer has switched back to it. i.e. the commanded velocity +profile experiences a large, discrete jump. In these cases, option 2 is very useful. +* The robot fails to generate the commanded velocity due to, for example, unmodelled inclines, carpets etc. In these cases, option 1 is very useful. diff --git a/config/velocity_smoother_params.yaml b/config/velocity_smoother_params.yaml index c04498b..d1e35ec 100644 --- a/config/velocity_smoother_params.yaml +++ b/config/velocity_smoother_params.yaml @@ -3,19 +3,21 @@ # - acceleration limits are just low enough to avoid jerking velocity_smoother: ros__parameters: - # Mandatory parameters + # limits speed_lim_v: 0.8 speed_lim_w: 5.4 accel_lim_v: 0.3 accel_lim_w: 3.5 - # Optional parameters - frequency: 20.0 + # multiply the acceleration limit by this to permit faster decellerations decel_factor: 1.0 - # Robot velocity feedback type: + # recompute smoothed velocities at this rate + frequency: 20.0 + + # feedback type: # 0 - none # 1 - odometry - # 2 - robot commands (cmd_vel) - robot_feedback: 2 + # 2 - actual commanded velocity (e.g. after it's been piped through a mux) + feedback: 2 diff --git a/include/velocity_smoother/velocity_smoother.hpp b/include/velocity_smoother/velocity_smoother.hpp index eeb8563..c8ddf51 100644 --- a/include/velocity_smoother/velocity_smoother.hpp +++ b/include/velocity_smoother/velocity_smoother.hpp @@ -53,7 +53,7 @@ class VelocitySmoother final : public rclcpp::Node NONE, ODOMETRY, COMMANDS - } robot_feedback_; /**< What source to use as robot velocity feedback */ + } feedback_; /**< What source to use as feedback for smoothed velocity calculations */ bool quiet_; /**< Quieten some warnings that are unavoidable because of velocity multiplexing. **/ double speed_lim_v_, accel_lim_v_, decel_lim_v_; diff --git a/package.xml b/package.xml index cf54dbe..0e6a7ac 100644 --- a/package.xml +++ b/package.xml @@ -29,6 +29,7 @@ launch_testing launch_testing_ament_cmake launch_testing_ros + python3-matplotlib ros2test diff --git a/src/velocity_smoother.cpp b/src/velocity_smoother.cpp index bda4832..87cf930 100644 --- a/src/velocity_smoother.cpp +++ b/src/velocity_smoother.cpp @@ -49,14 +49,14 @@ VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions & options) : rclcpp double frequency = this->declare_parameter("frequency", 20.0); quiet_ = this->declare_parameter("quiet", false); decel_factor_ = this->declare_parameter("decel_factor", 1.0); - int feedback = this->declare_parameter("robot_feedback", static_cast(NONE)); + int feedback = this->declare_parameter("feedback", static_cast(NONE)); if ((static_cast(feedback) < NONE) || (static_cast(feedback) > COMMANDS)) { throw std::runtime_error("Invalid robot feedback type. Valid options are 0 (NONE, default), 1 (ODOMETRY) and 2 (COMMANDS)"); } - robot_feedback_ = static_cast(feedback); + feedback_ = static_cast(feedback); // Mandatory parameters rclcpp::ParameterValue speed_v = this->declare_parameter( @@ -100,10 +100,10 @@ VelocitySmoother::VelocitySmoother(const rclcpp::NodeOptions & options) : rclcpp decel_lim_w_ = decel_factor_*accel_lim_w_; // Publishers and subscribers - odometry_sub_ = this->create_subscription("odometry", rclcpp::QoS(1), std::bind(&VelocitySmoother::odometryCB, this, std::placeholders::_1)); - current_vel_sub_ = this->create_subscription("robot_cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::robotVelCB, this, std::placeholders::_1)); - raw_in_vel_sub_ = this->create_subscription("raw_cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::velocityCB, this, std::placeholders::_1)); - smooth_vel_pub_ = this->create_publisher("smooth_cmd_vel", 1); + odometry_sub_ = this->create_subscription("~/feedback/odometry", rclcpp::QoS(1), std::bind(&VelocitySmoother::odometryCB, this, std::placeholders::_1)); + current_vel_sub_ = this->create_subscription("~/feedback/cmd_vel", rclcpp::QoS(1), std::bind(&VelocitySmoother::robotVelCB, this, std::placeholders::_1)); + raw_in_vel_sub_ = this->create_subscription("~/input", rclcpp::QoS(1), std::bind(&VelocitySmoother::velocityCB, this, std::placeholders::_1)); + smooth_vel_pub_ = this->create_publisher("~/smoothed", 1); period_ = 1.0 / frequency; timer_ = this->create_wall_timer(std::chrono::milliseconds(static_cast(period_ * 1000.0)), std::bind(&VelocitySmoother::timerCB, this)); @@ -156,7 +156,7 @@ void VelocitySmoother::velocityCB(const geometry_msgs::msg::Twist::SharedPtr msg void VelocitySmoother::odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg) { - if (robot_feedback_ == ODOMETRY) + if (feedback_ == ODOMETRY) { current_vel_ = msg->twist.twist; } @@ -166,7 +166,7 @@ void VelocitySmoother::odometryCB(const nav_msgs::msg::Odometry::SharedPtr msg) void VelocitySmoother::robotVelCB(const geometry_msgs::msg::Twist::SharedPtr msg) { - if (robot_feedback_ == COMMANDS) + if (feedback_ == COMMANDS) { current_vel_ = *msg; } @@ -207,7 +207,7 @@ void VelocitySmoother::timerCB() bool v_different_from_feedback = current_vel_.linear.x < v_deviation_lower_bound || current_vel_.linear.x > v_deviation_upper_bound; bool w_different_from_feedback = current_vel_.angular.z < w_deviation_lower_bound || current_vel_.angular.z > w_deviation_upper_bound; - if ((robot_feedback_ != NONE) && (input_active_ == true) && (cb_avg_time_ > 0.0) && + if ((feedback_ != NONE) && (input_active_ == true) && (cb_avg_time_ > 0.0) && (((this->get_clock()->now() - last_velocity_cb_time_).seconds() > 5.0*cb_avg_time_) || // 5 missing msgs v_different_from_feedback || w_different_from_feedback)) { @@ -218,7 +218,7 @@ void VelocitySmoother::timerCB() // this condition can be unavoidable due to preemption of current velocity control on // velocity multiplexer so be quiet if we're instructed to do so RCLCPP_WARN(get_logger(), "Velocity Smoother : using robot velocity feedback %s instead of last command: %f, %f, %f", - std::string(robot_feedback_ == ODOMETRY ? "odometry" : "end commands").c_str(), + std::string(feedback_ == ODOMETRY ? "odometry" : "end commands").c_str(), (this->get_clock()->now() - last_velocity_cb_time_).seconds(), current_vel_.linear.x - last_cmd_vel_linear_x_, current_vel_.angular.z - last_cmd_vel_angular_z_); @@ -239,7 +239,7 @@ void VelocitySmoother::timerCB() double v_inc, w_inc, max_v_inc, max_w_inc; v_inc = target_vel_.linear.x - last_cmd_vel_linear_x_; - if ((robot_feedback_ == ODOMETRY) && (current_vel_.linear.x*target_vel_.linear.x < 0.0)) + if ((feedback_ == ODOMETRY) && (current_vel_.linear.x*target_vel_.linear.x < 0.0)) { // countermarch (on robots with significant inertia; requires odometry feedback to be detected) max_v_inc = decel_lim_v_*period_; @@ -250,7 +250,7 @@ void VelocitySmoother::timerCB() } w_inc = target_vel_.angular.z - last_cmd_vel_angular_z_; - if ((robot_feedback_ == ODOMETRY) && (current_vel_.angular.z*target_vel_.angular.z < 0.0)) + if ((feedback_ == ODOMETRY) && (current_vel_.angular.z*target_vel_.angular.z < 0.0)) { // countermarch (on robots with significant inertia; requires odometry feedback to be detected) max_w_inc = decel_lim_w_*period_; @@ -341,10 +341,10 @@ rcl_interfaces::msg::SetParametersResult VelocitySmoother::parameterUpdate( decel_factor_ = parameter.get_value(); } - else if (parameter.get_name() == "robot_feedback") + else if (parameter.get_name() == "feedback") { result.successful = false; - result.reason = "robot_feedback cannot be changed on-the-fly"; + result.reason = "feedback cannot be changed on-the-fly"; break; } else if (parameter.get_name() == "speed_lim_v") diff --git a/test/plot b/test/plot deleted file mode 100755 index 4831d69..0000000 --- a/test/plot +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# Copyright (c) 2020 Daniel Stonier -# -# License: BSD -# https://raw.githubusercontent.com/kobuki-base/velocity_smoother/license/LICENSE -# -############################################################################## -# Imports -############################################################################## - -import matplotlib -# choose a backend that lets it construct plots off the main thread -# https://stackoverflow.com/questions/49921721/runtimeerror-main-thread-is-not-in-main-loop-with-matplotlib-and-flask -# matplotlib.use('Agg') - -import matplotlib.pyplot as plt - -############################################################################## -# Helpers -############################################################################## - -def input_velocities(): - return [ - 0.0, 0.02, 0.04, 0.06, 0.08, 0.1, 0.12000000000000001, 0.14, 0.16, 0.18, 0.19999999999999998, 0.21999999999999997, 0.23999999999999996, 0.25999999999999995, 0.27999999999999997, 0.3, 0.32, 0.34, 0.36000000000000004, 0.38000000000000006, 0.4000000000000001, 0.4200000000000001, 0.4400000000000001, 0.46000000000000013, 0.48000000000000015, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.5000000000000001, 0.4800000000000001, 0.4600000000000001, 0.44000000000000006, 0.42000000000000004, 0.4, 0.38, 0.36, 0.33999999999999997, 0.31999999999999995, 0.29999999999999993, 0.2799999999999999, 0.2599999999999999, 0.2399999999999999, 0.21999999999999992, 0.19999999999999993, 0.17999999999999994, 0.15999999999999995, 0.13999999999999996, 0.11999999999999995, 0.09999999999999995, 0.07999999999999995, 0.05999999999999994, 0.03999999999999994, 0.019999999999999938, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 - ] - -def input_timestamps(): - return [ - 261880.14221302, 261880.142892134, 261880.14353241, 261880.144190648, 261880.144794521, 261880.145393436, 261880.145964599, 261880.146470781, 261880.227405694, 261880.32708237, 261880.428778702, 261880.527400389, 261880.628396705, 261880.727179625, 261880.827596018, 261880.928719466, 261881.02717419, 261881.127425582, 261881.228316766, 261881.32719927, 261881.427290684, 261881.528432727, 261881.627841602, 261881.727233501, 261881.828316796, 261881.927835009, 261882.027396433, 261882.128344526, 261882.227708809, 261882.326924882, 261882.428335065, 261882.527395607, 261882.627305028, 261882.728653235, 261882.827632936, 261882.92726536, 261883.029947771, 261883.127782893, 261883.227358342, 261883.327944942, 261883.427711053, 261883.526905507, 261883.628679135, 261883.727385809, 261883.82729181, 261883.929806366, 261884.027014665, 261884.126843845, 261884.228120745, 261884.327872441, 261884.427827186, 261884.528157522, 261884.627380248, 261884.727292132, 261884.827215204, 261884.92675076, 261885.026913513, 261885.127885119, 261885.227778042, 261885.326718498, 261885.427191971, 261885.527549385, 261885.62745424, 261885.728294264, 261885.82733983, 261885.927442733, 261886.027311687, 261886.127009309, 261886.226973691, 261886.326995634, 261886.427487884, 261886.527595402, 261886.629489327, 261886.727109456, 261886.828159841, 261886.929614432, 261887.027448246, 261887.127141029, 261887.22758736, 261887.32780362, 261887.427760148, 261887.527719469, 261887.627758817, 261887.727750756, 261887.826923689, 261887.9274633, 261888.027380409, 261888.127326321, 261888.228129794, 261888.32690823 - ] - -def smoothed_velocities(): - return [ - 0.035, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.019999999999999938, 0.0, 0.0049999999999999385, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.045, 0.015, 0.03, 0.015, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 - ] - -def smoothed_timestamps(): - return [ - 261880.14240701, 261880.143072453, 261880.143674696, 261880.144346175, 261880.144936336, 261880.145534263, 261880.146101053, 261880.146548987, 261880.146786922, 261880.147022127, 261880.176769071, 261880.226671562, 261880.276837923, 261880.326667756, 261880.376793417, 261880.42668658, 261880.477022717, 261880.526693616, 261880.57678769, 261880.627466054, 261880.676408721, 261880.72649954, 261880.776686612, 261880.826761201, 261880.876696647, 261880.927817491, 261880.976761817, 261881.026499023, 261881.076745071, 261881.126507614, 261881.176535952, 261881.227368058, 261881.276472553, 261881.326536068, 261881.377249934, 261881.426512034, 261881.476525641, 261881.527360445, 261881.576730529, 261881.626668984, 261881.677087165, 261881.72660592, 261881.776624475, 261881.827370378, 261881.87665505, 261881.926781003, 261881.976623761, 261882.026621401, 261882.076708664, 261882.127586894, 261882.176495409, 261882.226723918, 261882.277370879, 261882.326447541, 261882.376620294, 261882.427415244, 261882.476553046, 261882.52659096, 261882.576920259, 261882.626632503, 261882.676624976, 261882.727752373, 261882.776668009, 261882.826709858, 261882.877109511, 261882.92657211, 261882.976656427, 261883.02763568, 261883.076757144, 261883.126834561, 261883.177104524, 261883.226648128, 261883.276669605, 261883.327357508, 261883.376687199, 261883.42672946, 261883.477205377, 261883.526427034, 261883.576744167, 261883.627923911, 261883.676504628, 261883.726569261, 261883.777082846, 261883.826585742, 261883.876589474, 261883.927603448, 261883.976600536, 261884.026393361, 261884.077137237, 261884.126288006, 261884.176326186, 261884.227194614, 261884.276436638, 261884.326455566, 261884.376743419, 261884.426784071, 261884.476430174, 261884.527341704, 261884.576447973, 261884.626594742, 261884.676645187, 261884.726627471, 261884.776325834, 261884.826736691, 261884.876451995, 261884.926227515, 261884.976989186, 261885.026437844, 261885.076558816, 261885.127208764, 261885.176612875, 261885.226899957, 261885.276693446, 261885.326248212, 261885.376450881, 261885.426457985, 261885.476512412, 261885.526621705, 261885.576496734, 261885.626599946, 261885.676413363, 261885.726597832, 261885.776472841, 261885.826607733, 261885.876511041, 261885.926653559, 261885.976543847, 261886.02655483, 261886.076374163, 261886.126331283, 261886.176504341, 261886.226279104, 261886.276774294, 261886.326493651, 261886.376577506, 261886.426635981, 261886.47667328, 261886.526633198, 261886.576499157, 261886.627321444, 261886.676175858, 261886.726351107, 261886.776558054, 261886.826509439, 261886.876519881, 261886.927582791, 261886.976570317, 261887.026639537, 261887.07647044, 261887.126564955, 261887.176205726, 261887.226860381, 261887.276404165, 261887.32677281, 261887.376722071, 261887.426748946, 261887.476497593, 261887.52662691, 261887.576381643, 261887.626684344, 261887.676477476, 261887.726682272, 261887.77656108, 261887.82619788, 261887.876596594, 261887.926593102, 261887.976470201, 261888.026599103, 261888.076567559, 261888.126406715, 261888.176464492, 261888.226496902, 261888.276522296, 261888.32640945, 261888.376314075, 261888.42620868 - ] - -############################################################################## -# Tests -############################################################################## - -if __name__ == "__main__": - plt.plot(input_timestamps(), input_velocities(), label="input") - plt.plot(smoothed_timestamps(), smoothed_velocities(), label="smooth") - plt.xlabel('time') - plt.ylabel('velocity') - plt.title("Raw Input vs Smoothed Velocities") - plt.legend() - plt.show() diff --git a/test/test_translational_smoothing.py b/test/test_translational_smoothing.py index 48ff05a..0d7d327 100755 --- a/test/test_translational_smoothing.py +++ b/test/test_translational_smoothing.py @@ -51,9 +51,9 @@ def create_command_profile_node(): output='both', emulate_tty=True, remappings=[ - ('~/cmd_vel', '/raw_cmd_vel'), + ('~/actual/cmd_vel', '/cmd_vel'), + ('~/generated/cmd_vel', '~/cmd_vel'), ('~/odometry', '/odometry'), - ('~/actual_cmd_vel', '/smooth_cmd_vel') ], ) @@ -67,7 +67,7 @@ def create_velocity_smoother_node(): parameters['accel_lim_w'] = 3.5 parameters['frequency'] = 20.0 parameters['decel_factor'] = 2.0 - parameters['robot_feedback'] = 1 # 1 - ODOMETRY, 2 - COMMANDS (velocity_smoother.hpp) + parameters['feedback'] = 1 # 1 - ODOMETRY, 2 - COMMANDS (velocity_smoother.hpp) return launch_ros.actions.Node( package='velocity_smoother', node_executable='velocity_smoother', @@ -75,7 +75,10 @@ def create_velocity_smoother_node(): output='both', parameters=[parameters], remappings=[ - ('robot_cmd_vel', 'smooth_cmd_vel') + ('~/feedback/odometry', '/odometry'), + ('~/feedback/cmd_vel', '/cmd_vel'), + ('~/input', '/commands/cmd_vel'), + ('~/smoothed', '/cmd_vel'), ], ) @@ -133,13 +136,13 @@ def received_smoothed_data(msg): input_subscriber = node.create_subscription( geometry_msgs.msg.Twist, - 'raw_cmd_vel', + '/commands/cmd_vel', received_input_data, 10, ) smoothed_subscriber = node.create_subscription( geometry_msgs.msg.Twist, - 'smooth_cmd_vel', + '/cmd_vel', received_smoothed_data, 10, ) diff --git a/test/translational_command_profile.py b/test/translational_command_profile.py index 0c78ea7..1ebe7a4 100755 --- a/test/translational_command_profile.py +++ b/test/translational_command_profile.py @@ -127,7 +127,7 @@ def __init__(self, node_name): self.node = rclpy.create_node(node_name) self.cmd_vel_publisher = self.node.create_publisher( msg_type=geometry_msgs.Twist, - topic="~/cmd_vel", + topic="~/generated/cmd_vel", qos_profile=10, ) self.timer = self.node.create_timer( @@ -135,15 +135,16 @@ def __init__(self, node_name): callback=self.update_and_publish_command ) + # tune into the actual cmd_vel (after smoothing/muxing) to generate odometry updates self.actual_cmd_vel_subscriber = self.node.create_subscription( geometry_msgs.Twist, - '~/actual_cmd_vel', + '~/actual/cmd_vel', self.update_and_publish_odometry, 10, ) self.odom_publisher = self.node.create_publisher( - msg_type=nav_msgs.Odometry, - topic="~/odometry", + msg_type=nav_msgs.Odometry, + topic="~/odometry", qos_profile=10, ) @@ -182,7 +183,7 @@ def shutdown(self): banner("Command Profile") rclpy.init() - engine = ExecutionEngine(node_name="publisher") + engine = ExecutionEngine(node_name="commands") executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(engine.node)