Skip to content

Commit add09f6

Browse files
SteveMacenskiSteveMacenski
authored
Migrating twist to twiststamped in simulations for recommended default bringups (#4779)
* migrating from twist to twiststamped Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bump ci cache for updated TB4 sim files Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing collision monitor, velocity smoother unit tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix assisted teleop test Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fixing docking server smoke test Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * bust nav2 minimal tb sim cache --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>
1 parent 9284c8a commit add09f6

File tree

12 files changed

+40
-34
lines changed

12 files changed

+40
-34
lines changed

‎.circleci/config.yml

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -33,12 +33,12 @@ _commands:
3333
- restore_cache:
3434
name: Restore Cache << parameters.key >>
3535
keys:
36-
- "<< parameters.key >>-v29\
36+
- "<< parameters.key >>-v31\
3737
-{{ arch }}\
3838
-{{ .Branch }}\
3939
-{{ .Environment.CIRCLE_PR_NUMBER }}\
4040
-{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}"
41-
- "<< parameters.key >>-v29\
41+
- "<< parameters.key >>-v31\
4242
-{{ arch }}\
4343
-main\
4444
-<no value>\
@@ -58,7 +58,7 @@ _commands:
5858
steps:
5959
- save_cache:
6060
name: Save Cache << parameters.key >>
61-
key: "<< parameters.key >>-v29\
61+
key: "<< parameters.key >>-v31\
6262
-{{ arch }}\
6363
-{{ .Branch }}\
6464
-{{ .Environment.CIRCLE_PR_NUMBER }}\

‎nav2_collision_monitor/test/collision_monitor_node_test.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -220,6 +220,7 @@ class Tester : public ::testing::Test
220220
Tester::Tester()
221221
{
222222
cm_ = std::make_shared<CollisionMonitorWrapper>();
223+
cm_->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
223224

224225
footprint_pub_ = cm_->create_publisher<geometry_msgs::msg::PolygonStamped>(
225226
FOOTPRINT_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());

‎nav2_docking/opennav_docking/test/test_docking_server.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
import unittest
1818

1919
from action_msgs.msg import GoalStatus
20-
from geometry_msgs.msg import TransformStamped, Twist
20+
from geometry_msgs.msg import TransformStamped, Twist, TwistStamped
2121
from launch import LaunchDescription
2222
from launch_ros.actions import Node
2323
import launch_testing
@@ -90,8 +90,8 @@ def tearDownClass(cls):
9090
rclpy.shutdown()
9191

9292
def command_velocity_callback(self, msg):
93-
self.node.get_logger().info('Command: %f %f' % (msg.linear.x, msg.angular.z))
94-
self.command = msg
93+
self.node.get_logger().info('Command: %f %f' % (msg.twist.linear.x, msg.twist.angular.z))
94+
self.command = msg.twist
9595

9696
def timer_callback(self):
9797
# Propagate command
@@ -155,7 +155,7 @@ def test_docking_server(self):
155155

156156
# Subscribe to command velocity
157157
self.node.create_subscription(
158-
Twist,
158+
TwistStamped,
159159
'cmd_vel',
160160
self.command_velocity_callback,
161161
10

‎nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,7 @@ def __init__(self):
7474
self.declare_parameter('scan_frame_id', 'base_scan')
7575
self.scan_frame_id = self.get_parameter('scan_frame_id').get_parameter_value().string_value
7676

77-
self.declare_parameter('enable_stamped_cmd_vel', False)
77+
self.declare_parameter('enable_stamped_cmd_vel', True)
7878
use_stamped = self.get_parameter('enable_stamped_cmd_vel').get_parameter_value().bool_value
7979

8080
self.declare_parameter('scan_publish_dur', 0.1)

‎nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.cpp

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -54,13 +54,13 @@ AssistedTeleopBehaviorTester::AssistedTeleopBehaviorTester()
5454
node_->create_publisher<std_msgs::msg::Empty>("preempt_teleop", 10);
5555

5656
cmd_vel_pub_ =
57-
node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_teleop", 10);
57+
node_->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel_teleop", 10);
5858

5959
subscription_ = node_->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
6060
"amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
6161
std::bind(&AssistedTeleopBehaviorTester::amclPoseCallback, this, std::placeholders::_1));
6262

63-
filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::Twist>(
63+
filtered_vel_sub_ = node_->create_subscription<geometry_msgs::msg::TwistStamped>(
6464
"cmd_vel",
6565
rclcpp::SystemDefaultsQoS(),
6666
std::bind(&AssistedTeleopBehaviorTester::filteredVelCallback, this, std::placeholders::_1));
@@ -167,9 +167,9 @@ bool AssistedTeleopBehaviorTester::defaultAssistedTeleopTest(
167167
counter_ = 0;
168168
auto start_time = std::chrono::system_clock::now();
169169
while (rclcpp::ok()) {
170-
geometry_msgs::msg::Twist cmd_vel = geometry_msgs::msg::Twist();
171-
cmd_vel.linear.x = lin_vel;
172-
cmd_vel.angular.z = ang_vel;
170+
geometry_msgs::msg::TwistStamped cmd_vel = geometry_msgs::msg::TwistStamped();
171+
cmd_vel.twist.linear.x = lin_vel;
172+
cmd_vel.twist.angular.z = ang_vel;
173173
cmd_vel_pub_->publish(cmd_vel);
174174

175175
if (counter_ > 1) {
@@ -265,9 +265,9 @@ void AssistedTeleopBehaviorTester::amclPoseCallback(
265265
}
266266

267267
void AssistedTeleopBehaviorTester::filteredVelCallback(
268-
geometry_msgs::msg::Twist::SharedPtr msg)
268+
geometry_msgs::msg::TwistStamped::SharedPtr msg)
269269
{
270-
if (msg->linear.x == 0.0f) {
270+
if (msg->twist.linear.x == 0.0f) {
271271
counter_++;
272272
} else {
273273
counter_ = 0;

‎nav2_system_tests/src/behaviors/assisted_teleop/assisted_teleop_behavior_tester.hpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424

2525
#include "geometry_msgs/msg/pose_stamped.hpp"
2626
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
27-
#include "geometry_msgs/msg/twist.hpp"
27+
#include "geometry_msgs/msg/twist_stamped.hpp"
2828
#include "geometry_msgs/msg/pose2_d.hpp"
2929
#include "nav2_costmap_2d/costmap_topic_collision_checker.hpp"
3030
#include "nav2_msgs/action/assisted_teleop.hpp"
@@ -68,7 +68,7 @@ class AssistedTeleopBehaviorTester
6868

6969
void amclPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr);
7070

71-
void filteredVelCallback(geometry_msgs::msg::Twist::SharedPtr msg);
71+
void filteredVelCallback(geometry_msgs::msg::TwistStamped::SharedPtr msg);
7272

7373
unsigned int counter_;
7474
bool is_active_;
@@ -83,11 +83,11 @@ class AssistedTeleopBehaviorTester
8383
// Publishers
8484
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
8585
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr preempt_pub_;
86-
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
86+
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_pub_;
8787

8888
// Subscribers
8989
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
90-
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr filtered_vel_sub_;
90+
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr filtered_vel_sub_;
9191

9292
// Action client to call AssistedTeleop action
9393
rclcpp_action::Client<AssistedTeleop>::SharedPtr client_ptr_;

‎nav2_util/include/nav2_util/twist_publisher.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,7 @@ class TwistPublisher
6161
using nav2_util::declare_parameter_if_not_declared;
6262
declare_parameter_if_not_declared(
6363
node, "enable_stamped_cmd_vel",
64-
rclcpp::ParameterValue{false});
64+
rclcpp::ParameterValue{true});
6565
node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
6666
if (is_stamped_) {
6767
twist_stamped_pub_ = node->create_publisher<geometry_msgs::msg::TwistStamped>(
@@ -122,7 +122,7 @@ class TwistPublisher
122122

123123
protected:
124124
std::string topic_;
125-
bool is_stamped_;
125+
bool is_stamped_{true};
126126
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
127127
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::TwistStamped>::SharedPtr
128128
twist_stamped_pub_;

‎nav2_util/include/nav2_util/twist_subscriber.hpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -92,7 +92,7 @@ class TwistSubscriber
9292
{
9393
nav2_util::declare_parameter_if_not_declared(
9494
node, "enable_stamped_cmd_vel",
95-
rclcpp::ParameterValue(false));
95+
rclcpp::ParameterValue(true));
9696
node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
9797
if (is_stamped_) {
9898
twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
@@ -125,7 +125,7 @@ class TwistSubscriber
125125
{
126126
nav2_util::declare_parameter_if_not_declared(
127127
node, "enable_stamped_cmd_vel",
128-
rclcpp::ParameterValue(false));
128+
rclcpp::ParameterValue(true));
129129
node->get_parameter("enable_stamped_cmd_vel", is_stamped_);
130130
if (is_stamped_) {
131131
twist_stamped_sub_ = node->create_subscription<geometry_msgs::msg::TwistStamped>(
@@ -140,7 +140,7 @@ class TwistSubscriber
140140

141141
protected:
142142
//! @brief The user-configured value for ROS parameter enable_stamped_cmd_vel
143-
bool is_stamped_{false};
143+
bool is_stamped_{true};
144144
//! @brief The subscription when using Twist
145145
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_sub_ {nullptr};
146146
//! @brief The subscription when using TwistStamped

‎nav2_util/test/test_twist_publisher.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ TEST(TwistPublisher, Unstamped)
2929
rclcpp::init(0, nullptr);
3030
auto pub_node = std::make_shared<nav2_util::LifecycleNode>("pub_node", "");
3131
pub_node->configure();
32+
pub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
3233
auto vel_publisher = std::make_unique<nav2_util::TwistPublisher>(pub_node, "cmd_vel", 1);
3334
ASSERT_EQ(vel_publisher->get_subscription_count(), 0);
3435
EXPECT_FALSE(vel_publisher->is_activated());

‎nav2_util/test/test_twist_subscriber.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ TEST(TwistSubscriber, Unstamped)
2929
auto sub_node = std::make_shared<nav2_util::LifecycleNode>("sub_node", "");
3030
sub_node->configure();
3131
sub_node->activate();
32+
sub_node->declare_parameter("enable_stamped_cmd_vel", rclcpp::ParameterValue(false));
3233

3334
geometry_msgs::msg::TwistStamped sub_msg {};
3435
auto vel_subscriber = std::make_unique<nav2_util::TwistSubscriber>(

‎nav2_velocity_smoother/test/test_velocity_smoother.cpp

Lines changed: 12 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
#include "rclcpp/rclcpp.hpp"
2323
#include "nav2_velocity_smoother/velocity_smoother.hpp"
2424
#include "nav_msgs/msg/odometry.hpp"
25-
#include "geometry_msgs/msg/twist.hpp"
25+
#include "geometry_msgs/msg/twist_stamped.hpp"
2626
#include "nav2_util/twist_subscriber.hpp"
2727

2828
using namespace std::chrono_literals;
@@ -53,7 +53,10 @@ class VelSmootherShim : public nav2_velocity_smoother::VelocitySmoother
5353
bool hasCommandMsg() {return last_command_time_.nanoseconds() != 0;}
5454
geometry_msgs::msg::TwistStamped::SharedPtr lastCommandMsg() {return command_;}
5555

56-
void sendCommandMsg(geometry_msgs::msg::Twist::SharedPtr msg) {inputCommandCallback(msg);}
56+
void sendCommandMsg(geometry_msgs::msg::TwistStamped::SharedPtr msg)
57+
{
58+
inputCommandStampedCallback(msg);
59+
}
5760
};
5861

5962
TEST(VelocitySmootherTest, openLoopTestTimer)
@@ -81,8 +84,8 @@ TEST(VelocitySmootherTest, openLoopTestTimer)
8184
});
8285

8386
// Send a velocity command
84-
auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
85-
cmd->linear.x = 1.0; // Max is 0.5, so should threshold
87+
auto cmd = std::make_shared<geometry_msgs::msg::TwistStamped>();
88+
cmd->twist.linear.x = 1.0; // Max is 0.5, so should threshold
8689
smoother->sendCommandMsg(cmd);
8790

8891
// Process velocity smoothing and send updated odometry based on commands
@@ -147,8 +150,8 @@ TEST(VelocitySmootherTest, approxClosedLoopTestTimer)
147150
}
148151

149152
// Send a velocity command
150-
auto cmd = std::make_shared<geometry_msgs::msg::Twist>();
151-
cmd->linear.x = 1.0; // Max is 0.5, so should threshold
153+
auto cmd = std::make_shared<geometry_msgs::msg::TwistStamped>();
154+
cmd->twist.linear.x = 1.0; // Max is 0.5, so should threshold
152155
smoother->sendCommandMsg(cmd);
153156

154157
// Process velocity smoothing and send updated odometry based on commands
@@ -568,10 +571,10 @@ TEST(VelocitySmootherTest, testCommandCallback)
568571
smoother->configure(state);
569572
smoother->activate(state);
570573

571-
auto pub = smoother->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
574+
auto pub = smoother->create_publisher<geometry_msgs::msg::TwistStamped>("cmd_vel", 1);
572575
pub->on_activate();
573-
auto msg = std::make_unique<geometry_msgs::msg::Twist>();
574-
msg->linear.x = 100.0;
576+
auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
577+
msg->twist.linear.x = 100.0;
575578
pub->publish(std::move(msg));
576579
rclcpp::spin_some(smoother->get_node_base_interface());
577580

‎tools/underlay.repos

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,4 +34,4 @@ repositories:
3434
ros-navigation/nav2_minimal_turtlebot_simulation:
3535
type: git
3636
url: https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation.git
37-
version: f0eeedbc95d9f7cc8a513f7d46a84b3d08a3d395
37+
version: 091b5ff12436890a54de6325df3573ae110e912b

0 commit comments

Comments
 (0)