diff --git a/tricycle_controller/config/tricycle_drive_controller.yaml b/tricycle_controller/config/tricycle_drive_controller.yaml index a7853954d7..e5b18f9d36 100644 --- a/tricycle_controller/config/tricycle_drive_controller.yaml +++ b/tricycle_controller/config/tricycle_drive_controller.yaml @@ -35,7 +35,7 @@ tricycle_controller: # max_acceleration: 1000.0 # cmd_vel input - cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received + cmd_vel_timeout: 0.5 # In seconds. Timeout to stop if no cmd_vel is received # Debug publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. diff --git a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp index b8aa29e92b..6e3eda87f2 100644 --- a/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp +++ b/tricycle_controller/include/tricycle_controller/tricycle_controller.hpp @@ -128,7 +128,7 @@ class TricycleController : public controller_interface::ControllerInterface tf2_msgs::msg::TFMessage tf_msg_; // Timeout to consider cmd_vel commands old - std::chrono::milliseconds cmd_vel_timeout_{500}; + rclcpp::Duration cmd_vel_timeout_ = rclcpp::Duration::from_seconds(0.5); bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr velocity_command_subscriber_ = nullptr; diff --git a/tricycle_controller/src/tricycle_controller.cpp b/tricycle_controller/src/tricycle_controller.cpp index c61d781919..d3636602c9 100644 --- a/tricycle_controller/src/tricycle_controller.cpp +++ b/tricycle_controller/src/tricycle_controller.cpp @@ -256,7 +256,7 @@ CallbackReturn TricycleController::on_configure(const rclcpp_lifecycle::State & odometry_.setWheelParams(params_.wheelbase, params_.wheel_radius); odometry_.setVelocityRollingWindowSize(static_cast(params_.velocity_rolling_window_size)); - cmd_vel_timeout_ = std::chrono::milliseconds{params_.cmd_vel_timeout}; + cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout); params_.publish_ackermann_command = get_node()->get_parameter("publish_ackermann_command").as_bool(); diff --git a/tricycle_controller/src/tricycle_controller_parameter.yaml b/tricycle_controller/src/tricycle_controller_parameter.yaml index cf661eddc1..1b1a80a972 100644 --- a/tricycle_controller/src/tricycle_controller_parameter.yaml +++ b/tricycle_controller/src/tricycle_controller_parameter.yaml @@ -67,9 +67,9 @@ tricycle_controller: description: "for doing the pose integration in separate node.", } cmd_vel_timeout: { - type: int, - default_value: 500, # milliseconds - description: "Timeout in milliseconds, after which input command on ``cmd_vel`` topic is considered staled.", + type: double, + default_value: 0.5, # seconds + description: "Timeout in seconds, after which input command on ``cmd_vel`` topic is considered staled.", } publish_ackermann_command: { type: bool,