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

[TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break #468

Merged
merged 2 commits into from
Dec 4, 2022
Merged

[TricycleController] Removed “publish period” functionality ⏱ #abi-break #behavior-break #468

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
589c3f0
removed publish_period, publish_rate previous_publish_time_stamp
Nov 17, 2022
380c2dc
Updating formatting in tricycle controller.
destogl Dec 3, 2022
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
5 changes: 0 additions & 5 deletions tricycle_controller/include/tricycle_controller/tricycle_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -167,11 +167,6 @@ class TricycleController : public controller_interface::ControllerInterface
TractionLimiter limiter_traction_;
SteeringLimiter limiter_steering_;

// publish rate limiter
double publish_rate_ = 50.0;
rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
rclcpp::Time previous_publish_timestamp_{0};

bool is_halted = false;
bool use_stamped_vel_ = true;

Expand Down
61 changes: 25 additions & 36 deletions tricycle_controller/src/tricycle_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,6 @@ CallbackReturn TricycleController::on_init()
auto_declare<bool>("publish_ackermann_command", publish_ackermann_command_);
auto_declare<int>("velocity_rolling_window_size", 10);
auto_declare<bool>("use_stamped_vel", use_stamped_vel_);
auto_declare<double>("publish_rate", publish_rate_);

auto_declare<double>("traction.max_velocity", NAN);
auto_declare<double>("traction.min_velocity", NAN);
Expand Down Expand Up @@ -169,40 +168,35 @@ controller_interface::return_type TricycleController::update(
tf2::Quaternion orientation;
orientation.setRPY(0.0, 0.0, odometry_.getHeading());

if (previous_publish_timestamp_ + publish_period_ < time)
if (realtime_odometry_publisher_->trylock())
{
previous_publish_timestamp_ += publish_period_;

if (realtime_odometry_publisher_->trylock())
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.stamp = time;
if (!odom_params_.odom_only_twist)
{
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.stamp = time;
if (!odom_params_.odom_only_twist)
{
odometry_message.pose.pose.position.x = odometry_.getX();
odometry_message.pose.pose.position.y = odometry_.getY();
odometry_message.pose.pose.orientation.x = orientation.x();
odometry_message.pose.pose.orientation.y = orientation.y();
odometry_message.pose.pose.orientation.z = orientation.z();
odometry_message.pose.pose.orientation.w = orientation.w();
}
odometry_message.twist.twist.linear.x = odometry_.getLinear();
odometry_message.twist.twist.angular.z = odometry_.getAngular();
realtime_odometry_publisher_->unlockAndPublish();
odometry_message.pose.pose.position.x = odometry_.getX();
odometry_message.pose.pose.position.y = odometry_.getY();
odometry_message.pose.pose.orientation.x = orientation.x();
odometry_message.pose.pose.orientation.y = orientation.y();
odometry_message.pose.pose.orientation.z = orientation.z();
odometry_message.pose.pose.orientation.w = orientation.w();
}
odometry_message.twist.twist.linear.x = odometry_.getLinear();
odometry_message.twist.twist.angular.z = odometry_.getAngular();
realtime_odometry_publisher_->unlockAndPublish();
}

if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
{
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
transform.header.stamp = time;
transform.transform.translation.x = odometry_.getX();
transform.transform.translation.y = odometry_.getY();
transform.transform.rotation.x = orientation.x();
transform.transform.rotation.y = orientation.y();
transform.transform.rotation.z = orientation.z();
transform.transform.rotation.w = orientation.w();
realtime_odometry_transform_publisher_->unlockAndPublish();
}
if (odom_params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
{
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
transform.header.stamp = time;
transform.transform.translation.x = odometry_.getX();
transform.transform.translation.y = odometry_.getY();
transform.transform.rotation.x = orientation.x();
transform.transform.rotation.y = orientation.y();
transform.transform.rotation.z = orientation.z();
transform.transform.rotation.w = orientation.w();
realtime_odometry_transform_publisher_->unlockAndPublish();
}

// Compute wheel velocity and angle
Expand Down Expand Up @@ -305,9 +299,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
publish_ackermann_command_ = get_node()->get_parameter("publish_ackermann_command").as_bool();
use_stamped_vel_ = get_node()->get_parameter("use_stamped_vel").as_bool();

publish_rate_ = get_node()->get_parameter("publish_rate").as_double();
publish_period_ = rclcpp::Duration::from_seconds(1.0 / publish_rate_);

try
{
limiter_traction_ = TractionLimiter(
Expand Down Expand Up @@ -421,8 +412,6 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State &
odometry_message.header.frame_id = odom_params_.odom_frame_id;
odometry_message.child_frame_id = odom_params_.base_frame_id;

previous_publish_timestamp_ = get_node()->get_clock()->now();

// initialize odom values zeros
odometry_message.twist =
geometry_msgs::msg::TwistWithCovariance(rosidl_runtime_cpp::MessageInitialization::ALL);
Expand Down