Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[diff_drive_controller] Remove non-stamped Twist option #812

Merged
merged 2 commits into from
Nov 7, 2023
Merged

[diff_drive_controller] Remove non-stamped Twist option #812

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
973608c
[diff_drive_controller] Remove non-stamped Twist option
bmagyar Nov 1, 2023
4c70502
fix format
bmagyar Nov 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 0 additions & 3 deletions diff_drive_controller/doc/userdoc.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,6 @@ Subscribers
~/cmd_vel [geometry_msgs/msg/TwistStamped]
Velocity command for the controller, if ``use_stamped_vel=true``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored.

~/cmd_vel_unstamped [geometry_msgs::msg::Twist]
Velocity command for the controller, if ``use_stamped_vel=false``. The controller extracts the x component of the linear velocity and the z component of the angular velocity. Velocities on other components are ignored.


Publishers
,,,,,,,,,,,
Expand Down
3 changes: 0 additions & 3 deletions diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -127,8 +127,6 @@ class DiffDriveController : public controller_interface::ControllerInterface

bool subscriber_is_active_ = false;
rclcpp::Subscription<Twist>::SharedPtr velocity_command_subscriber_ = nullptr;
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr
velocity_command_unstamped_subscriber_ = nullptr;

realtime_tools::RealtimeBox<std::shared_ptr<Twist>> received_velocity_msg_ptr_{nullptr};

Expand All @@ -151,7 +149,6 @@ class DiffDriveController : public controller_interface::ControllerInterface
rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};

bool is_halted = false;
bool use_stamped_vel_ = true;

bool reset();
void halt();
Expand Down
64 changes: 18 additions & 46 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
namespace
{
constexpr auto DEFAULT_COMMAND_TOPIC = "~/cmd_vel";
constexpr auto DEFAULT_COMMAND_UNSTAMPED_TOPIC = "~/cmd_vel_unstamped";
constexpr auto DEFAULT_COMMAND_OUT_TOPIC = "~/cmd_vel_out";
constexpr auto DEFAULT_ODOMETRY_TOPIC = "~/odom";
constexpr auto DEFAULT_TRANSFORM_TOPIC = "/tf";
Expand Down Expand Up @@ -302,7 +301,6 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(

cmd_vel_timeout_ = std::chrono::milliseconds{static_cast<int>(params_.cmd_vel_timeout * 1000.0)};
publish_limited_velocity_ = params_.publish_limited_velocity;
use_stamped_vel_ = params_.use_stamped_vel;

limiter_linear_ = SpeedLimiter(
params_.linear.x.has_velocity_limits, params_.linear.x.has_acceleration_limits,
Expand Down Expand Up @@ -340,50 +338,25 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
previous_commands_.emplace(empty_twist);

// initialize command subscriber
if (use_stamped_vel_)
{
velocity_command_subscriber_ = get_node()->create_subscription<Twist>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<Twist> msg) -> void
velocity_command_subscriber_ = get_node()->create_subscription<Twist>(
DEFAULT_COMMAND_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<Twist> msg) -> void
{
if (!subscriber_is_active_)
{
if (!subscriber_is_active_)
{
RCLCPP_WARN(
get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
return;
}
if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))
{
RCLCPP_WARN_ONCE(
get_node()->get_logger(),
"Received TwistStamped with zero timestamp, setting it to current "
"time, this message will only be shown once");
msg->header.stamp = get_node()->get_clock()->now();
}
received_velocity_msg_ptr_.set(std::move(msg));
});
}
else
{
velocity_command_unstamped_subscriber_ =
get_node()->create_subscription<geometry_msgs::msg::Twist>(
DEFAULT_COMMAND_UNSTAMPED_TOPIC, rclcpp::SystemDefaultsQoS(),
[this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) -> void
{
if (!subscriber_is_active_)
{
RCLCPP_WARN(
get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
return;
}

// Write fake header in the stored stamped command
std::shared_ptr<Twist> twist_stamped;
received_velocity_msg_ptr_.get(twist_stamped);
twist_stamped->twist = *msg;
twist_stamped->header.stamp = get_node()->get_clock()->now();
});
}
RCLCPP_WARN(get_node()->get_logger(), "Can't accept new commands. subscriber is inactive");
return;
}
if ((msg->header.stamp.sec == 0) && (msg->header.stamp.nanosec == 0))
{
RCLCPP_WARN_ONCE(
get_node()->get_logger(),
"Received TwistStamped with zero timestamp, setting it to current "
"time, this message will only be shown once");
msg->header.stamp = get_node()->get_clock()->now();
}
received_velocity_msg_ptr_.set(std::move(msg));
});

// initialize odometry publisher and messasge
odometry_publisher_ = get_node()->create_publisher<nav_msgs::msg::Odometry>(
Expand Down Expand Up @@ -534,7 +507,6 @@ bool DiffDriveController::reset()

subscriber_is_active_ = false;
velocity_command_subscriber_.reset();
velocity_command_unstamped_subscriber_.reset();

received_velocity_msg_ptr_.set(nullptr);
is_halted = false;
Expand Down
5 changes: 0 additions & 5 deletions diff_drive_controller/src/diff_drive_controller_parameter.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,11 +99,6 @@ diff_drive_controller:
default_value: 10,
description: "Size of the rolling window for calculation of mean velocity use in odometry.",
}
use_stamped_vel: {
type: bool,
default_value: true,
description: "Use stamp from input velocity message to calculate how old the command actually is.",
}
publish_rate: {
type: double,
default_value: 50.0, # Hz
Expand Down
Loading