Skip to content

Commit

Permalink
removed publish_period, publish_rate previous_publish_time_stamp
Browse files Browse the repository at this point in the history
  • Loading branch information
Robotgir committed Nov 17, 2022
1 parent 7b448c9 commit 4c938e9
Show file tree
Hide file tree
Showing 2 changed files with 0 additions and 15 deletions.
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
10 changes: 0 additions & 10 deletions tricycle_controller/src/tricycle_controller.cpp
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,9 +168,6 @@ controller_interface::return_type TricycleController::update(
tf2::Quaternion orientation;
orientation.setRPY(0.0, 0.0, odometry_.getHeading());

if (previous_publish_timestamp_ + publish_period_ < time)
{
previous_publish_timestamp_ += publish_period_;

if (realtime_odometry_publisher_->trylock())
{
Expand Down Expand Up @@ -203,7 +199,6 @@ controller_interface::return_type TricycleController::update(
transform.transform.rotation.w = orientation.w();
realtime_odometry_transform_publisher_->unlockAndPublish();
}
}

// Compute wheel velocity and angle
auto [alpha_write, Ws_write] = twist_to_ackermann(linear_command, angular_command);
Expand Down Expand Up @@ -305,9 +300,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 +413,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

0 comments on commit 4c938e9

Please sign in to comment.