From 6c9df8ea8db1cf49d6e6ec62e3e9baad1471bed4 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Sun, 21 Apr 2024 16:06:36 +0200 Subject: [PATCH 1/3] Added pid controller node for angular and linear velocity Controller to test Added feedforward changed to correct odom topic added reconfigurable EKF empty integrator fixed bug and added pure feedforward updated default PID values fixed pure feedforward --- smb/launch/smb.launch | 6 - smb_control/CMakeLists.txt | 30 ++- smb_control/config/PIDConfig.cfg | 27 +++ smb_control/config/localization.yaml | 53 ---- .../include/smb_control/PIDControllerNode.hpp | 114 +++++++++ smb_control/launch/smb_control.launch | 16 +- .../launch/start_ekf_reconfigure.launch | 7 + smb_control/package.xml | 7 + smb_control/src/PIDControllerNode.cpp | 228 ++++++++++++++++++ smb_control/src/ekf_restarter.py | 61 +++++ smb_gazebo/launch/sim.launch | 2 - 11 files changed, 478 insertions(+), 73 deletions(-) create mode 100755 smb_control/config/PIDConfig.cfg delete mode 100644 smb_control/config/localization.yaml create mode 100644 smb_control/include/smb_control/PIDControllerNode.hpp create mode 100644 smb_control/launch/start_ekf_reconfigure.launch create mode 100644 smb_control/src/PIDControllerNode.cpp create mode 100755 smb_control/src/ekf_restarter.py diff --git a/smb/launch/smb.launch b/smb/launch/smb.launch index 146ecd6..a944536 100644 --- a/smb/launch/smb.launch +++ b/smb/launch/smb.launch @@ -34,11 +34,6 @@ - - - - diff --git a/smb_control/CMakeLists.txt b/smb_control/CMakeLists.txt index 9f3f250..5d249be 100644 --- a/smb_control/CMakeLists.txt +++ b/smb_control/CMakeLists.txt @@ -2,25 +2,45 @@ cmake_minimum_required(VERSION 3.0.2) project(smb_control) find_package(catkin REQUIRED COMPONENTS + dynamic_reconfigure controller_manager joint_state_controller robot_state_publisher roslaunch + roscpp + std_msgs + geometry_msgs + sensor_msgs +) + +generate_dynamic_reconfigure_options( + config/PIDConfig.cfg ) catkin_package( -# INCLUDE_DIRS include -# LIBRARIES smb_control -# CATKIN_DEPENDS controller_manager joint_state_controller robot_state_publisher -# DEPENDS system_lib + INCLUDE_DIRS include + CATKIN_DEPENDS dynamic_reconfigure controller_manager joint_state_controller robot_state_publisher roscpp std_msgs geometry_msgs sensor_msgs dynamic_reconfigure ) include_directories( + include ${catkin_INCLUDE_DIRS} ) + +add_executable(pid_controller_node src/PIDControllerNode.cpp) +add_dependencies(pid_controller_node ${PROJECT_NAME}_gencfg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # Ensure the node is built after generating dynamic reconfigure headers + +target_link_libraries(pid_controller_node + ${EXTERNAL_LIBS} + ${catkin_LIBRARIES} +) + roslaunch_add_file_check(launch) -install(DIRECTORY config launch +install(DIRECTORY config launch include DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE ) + diff --git a/smb_control/config/PIDConfig.cfg b/smb_control/config/PIDConfig.cfg new file mode 100755 index 0000000..02a5a85 --- /dev/null +++ b/smb_control/config/PIDConfig.cfg @@ -0,0 +1,27 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * + +# Parameter generator handle +gen = ParameterGenerator() + +# Add double parameters for angular velocity control +# Name | Type | Level | Description | Default | Min | Max +gen.add("kp_angular", double_t, 0, "Proportional gain for angular velocity", 3.0, 0, 10) +gen.add("ki_angular", double_t, 0, "Integral gain for angular velocity", 1.0, 0, 5) +gen.add("kd_angular", double_t, 0, "Derivative gain for angular velocity", 0.0, 0, 1.0) + +# Add double parameters for linear velocity control +gen.add("kp_linear", double_t, 0, "Proportional gain for linear velocity", 1.5, 0, 10) +gen.add("ki_linear", double_t, 0, "Integral gain for linear velocity", 1.0, 0, 5) +gen.add("kd_linear", double_t, 0, "Derivative gain for linear velocity", 0.0, 0, 1.0) + +# Add boolean parameters for EKF sensor inputs +# Name | Type | Level | Description | Default +gen.add("use_imu", bool_t, 0, "Use IMU data", True) +gen.add("use_differential_drive_estimation", bool_t, 0, "Use Differential Drive Estimation data", True) +gen.add("use_vio", bool_t, 0, "Use VIO data", False) + +# Exit parameter generator +# Package Name | Node Name (used to generate documentation only) | cfg File Name +exit(gen.generate("smb_control", "PIDControllerNode", "PID")) diff --git a/smb_control/config/localization.yaml b/smb_control/config/localization.yaml deleted file mode 100644 index 4bfa41d..0000000 --- a/smb_control/config/localization.yaml +++ /dev/null @@ -1,53 +0,0 @@ -# see https://github.com/cra-ros-pkg/robot_localization/blob/noetic-devel/params/ekf_template.yaml -odom_frame: odom -base_link_frame: base_link -world_frame: odom - -two_d_mode: true - -frequency: 50 - -odom0: smb_diff_drive/odom -# publish_tf: true - -# x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az - -odom0_config: [false, false, false, - false, false, false, - true, true, true, - false, false, true, - false, false, false] - -odom0_queue_size: 100 -odom0_differential: false -odom0_relative: false - -imu: imu/data -imu_config: [false, false, false, - true, true, true, - false, false, false, - true, true, true, - false, false, false] -imu_differential: true -imu_queue_size: 10 -imu_remove_gravitational_acceleration: true - -# twist0: example/twist -# twist0_config: [false, false, false, -# false, false, false, -# true, true, true, -# false, false, false, -# false, false, false] -# twist0_queue_size: 3 -# twist0_rejection_threshold: 2 -# twist0_nodelay: false - - -# Acceleration and deceleration limits are not always the same for robots. -# acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] -# deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] - -# If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these gains -# acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] -# If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these gains -# deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] diff --git a/smb_control/include/smb_control/PIDControllerNode.hpp b/smb_control/include/smb_control/PIDControllerNode.hpp new file mode 100644 index 0000000..90c43aa --- /dev/null +++ b/smb_control/include/smb_control/PIDControllerNode.hpp @@ -0,0 +1,114 @@ +#ifndef PIDCONTROLLERNODE_HPP +#define PIDCONTROLLERNODE_HPP + +#include +#include + +#include +#include + +#include +#include + +class PIDController { +public: + PIDController(double kp = 0.0, double ki = 0.0, double kd = 0.0, double max_integral = 2.0) + : kp_(kp), ki_(ki), kd_(kd), integral_(0.0), prev_error_(0.0), max_integral_(max_integral) {} + + void setGains(double kp, double ki, double kd) { + kp_ = kp; + ki_ = ki; + kd_ = kd; + } + + void setMaxIntegral(double max_integral) { + max_integral_ = max_integral; + } + + double update(double error, double dt) { + if (dt <= 0.0001) return 0.0; // Prevent division by zero + + integral_ += error * dt; + + // Clamp the integral value to the maximum limit + if (integral_ > max_integral_) integral_ = max_integral_; + if (integral_ < -max_integral_) integral_ = -max_integral_; + + double derivative = (error - prev_error_) / dt; + prev_error_ = error; + + return kp_ * error + ki_ * integral_ + kd_ * derivative; + } + + void reset() { + integral_ = 0.0; + prev_error_ = 0.0; + } + + void resetIfZero(double cmd) { + if (std::abs(cmd) < 0.05) { // Check if the command is approximately zero + reset(); + } + } + +private: + double kp_, ki_, kd_; + double integral_, prev_error_; + double max_integral_; +}; + +class PIDControllerNode +{ +public: + // Constructor + PIDControllerNode(); + + // Destructor + ~PIDControllerNode(); + + // Main loop function + void run(); + + void configCallback(smb_control::PIDConfig &config, uint32_t level); + + void ekfConfigCallback(smb_control::PIDConfig &config, uint32_t level); + +private: + ros::NodeHandle nh_; + + // Publisher + ros::Publisher cmd_vel_pub_; + + // Subscriber + ros::Subscriber twist_sub_; + ros::Subscriber odom_sub_; + + bool pure_feed_forward_{true}; + + // Subscriber callback + void twistCallback(const geometry_msgs::Twist::ConstPtr& msg); + void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); + + // PID + PIDController angular_velocity_pid_; + PIDController linear_velocity_pid_; + bool use_diff_drive_estimation_{false}; + bool use_vio_{false}; + bool use_imu_{false}; + + std::shared_ptr> config_server_; + + geometry_msgs::Twist cmd_vel_requested_; + geometry_msgs::Twist cmd_vel_; + + ros::Time last_twist_time_; + ros::Time last_odom_time_; + double dt_{0.01}; + double odom_timeout_{0.5}; + double twist_timeout_{0.5}; + + void checkTimeout(); + void executeFeedforwardControl(); +}; + +#endif // PIDCONTROLLERNODE_HPP diff --git a/smb_control/launch/smb_control.launch b/smb_control/launch/smb_control.launch index 54e231a..9d9e49e 100644 --- a/smb_control/launch/smb_control.launch +++ b/smb_control/launch/smb_control.launch @@ -3,7 +3,6 @@ - @@ -51,16 +50,17 @@ - + + + + - - - - - + + + @@ -73,4 +73,6 @@ + + diff --git a/smb_control/launch/start_ekf_reconfigure.launch b/smb_control/launch/start_ekf_reconfigure.launch new file mode 100644 index 0000000..a34f98f --- /dev/null +++ b/smb_control/launch/start_ekf_reconfigure.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/smb_control/package.xml b/smb_control/package.xml index 7039431..459ac4b 100644 --- a/smb_control/package.xml +++ b/smb_control/package.xml @@ -18,6 +18,8 @@ joint_state_controller robot_state_publisher + dynamic_reconfigure + controller_manager joint_state_controller robot_state_publisher @@ -35,6 +37,11 @@ teleop_twist_keyboard twist_mux + roscpp + std_msgs + geometry_msgs + sensor_msgs + diff --git a/smb_control/src/PIDControllerNode.cpp b/smb_control/src/PIDControllerNode.cpp new file mode 100644 index 0000000..d3154a7 --- /dev/null +++ b/smb_control/src/PIDControllerNode.cpp @@ -0,0 +1,228 @@ +#include "smb_control/PIDControllerNode.hpp" +#include +#include + +PIDControllerNode::PIDControllerNode() +: nh_("~"), + last_odom_time_(ros::Time(0)), + angular_velocity_pid_(3.0, 0.0, 0.0), + linear_velocity_pid_(1.5, 1.0, 0.0), + + config_server_(std::make_shared>(nh_)), + odom_timeout_(0.1), // Timeout of 0.1 seconds + twist_timeout_(0.1) // Timeout of 0.1 seconds +{ + // Initialize the publisher + cmd_vel_pub_ = nh_.advertise("/control/smb_diff_drive/cmd_vel", 1); + + // Initialize the subscriber + twist_sub_ = nh_.subscribe("/control/twist_mux/output", 1, &PIDControllerNode::twistCallback, this); + odom_sub_ = nh_.subscribe("/odometry/filtered", 1, &PIDControllerNode::odomCallback, this); + + // Initialize dynamic reconfigure server for PID parameters + config_server_->setCallback(boost::bind(&PIDControllerNode::configCallback, this, _1, _2)); +} + +PIDControllerNode::~PIDControllerNode() +{ + // Cleanup can be handled here +} + +void PIDControllerNode::run() +{ + ros::Rate loop_rate(100); // 10 Hz + while (ros::ok()) + { + ros::spinOnce(); + checkTimeout(); + loop_rate.sleep(); + } +} + +void PIDControllerNode::configCallback(smb_control::PIDConfig &config, uint32_t level) +{ + // Update PID gains + angular_velocity_pid_.setGains(config.kp_angular, config.ki_angular, config.kd_angular); + linear_velocity_pid_.setGains(config.kp_linear, config.ki_linear, config.kd_linear); + + // Update EKF parameters + std::string ns = "/ekf_se"; // Ensure the namespace matches your EKF node's namespace + + // General EKF parameters + ros::param::set(ns + "/odom_frame", "odom"); + ros::param::set(ns + "/base_link_frame", "base_link"); + ros::param::set(ns + "/world_frame", "odom"); + ros::param::set(ns + "/two_d_mode", true); + ros::param::set(ns + "/frequency", 50); + ros::param::set(ns + "/publish_tf", true); + ros::param::set(ns + "/print_diagnostics", true); + + use_diff_drive_estimation_ = config.use_differential_drive_estimation; + use_vio_ = config.use_vio; + use_imu_ = config.use_imu; + + // Handle odom0 and odom1 logic + if (use_diff_drive_estimation_) + { + ros::param::set(ns + "/odom0", "/control/smb_diff_drive/odom"); + ros::param::set(ns + "/odom0_config", std::vector{false, false, false, false, false, false, true, true, true, false, false, true, false, false, false}); + ros::param::set(ns + "/odom0_queue_size", 100); + ros::param::set(ns + "/odom0_differential", false); + ros::param::set(ns + "/odom0_relative", false); + + if (use_vio_) + { + ros::param::set(ns + "/odom1", "/tracking_camera/odom/sample"); + ros::param::set(ns + "/odom1_config", std::vector{false, false, false, false, false, false, true, true, true, true, true, true, false, false, false}); + ros::param::set(ns + "/odom1_queue_size", 100); + ros::param::set(ns + "/odom1_differential", false); + ros::param::set(ns + "/odom1_relative", false); + } + else + { + ros::param::del(ns + "/odom1"); + ros::param::del(ns + "/odom1_config"); + ros::param::del(ns + "/odom1_queue_size"); + ros::param::del(ns + "/odom1_differential"); + ros::param::del(ns + "/odom1_relative"); + } + } + else if (use_vio_) + { + // If only VIO is used, configure it as odom0 + ros::param::set(ns + "/odom0", "/tracking_camera/odom/sample"); + ros::param::set(ns + "/odom0_config", std::vector{false, false, false, false, false, false, true, true, true, true, true, true, false, false, false}); + ros::param::set(ns + "/odom0_queue_size", 100); + ros::param::set(ns + "/odom0_differential", false); + ros::param::set(ns + "/odom0_relative", false); + + ros::param::del(ns + "/odom1"); + ros::param::del(ns + "/odom1_config"); + ros::param::del(ns + "/odom1_queue_size"); + ros::param::del(ns + "/odom1_differential"); + ros::param::del(ns + "/odom1_relative"); + } + else + { + // Neither differential drive nor VIO is used, delete both odom configurations + ros::param::del(ns + "/odom0"); + ros::param::del(ns + "/odom0_config"); + ros::param::del(ns + "/odom0_queue_size"); + ros::param::del(ns + "/odom0_differential"); + ros::param::del(ns + "/odom0_relative"); + + ros::param::del(ns + "/odom1"); + ros::param::del(ns + "/odom1_config"); + ros::param::del(ns + "/odom1_queue_size"); + ros::param::del(ns + "/odom1_differential"); + ros::param::del(ns + "/odom1_relative"); + + if(!use_imu_) + { + // If no sensors are used, delete the EKF node + pure_feed_forward_ = true; + } + } + + // IMU0 parameters + if (use_imu_) + { + ros::param::set(ns + "/imu0", "/imu"); + ros::param::set(ns + "/imu0_config", std::vector{false, false, false, true, true, true, false, false, false, true, true, true, false, false, false}); + ros::param::set(ns + "/imu0_differential", true); + ros::param::set(ns + "/imu0_queue_size", 10); + ros::param::set(ns + "/imu0_remove_gravitational_acceleration", true); + } + else + { + ros::param::del(ns + "/imu0"); + ros::param::del(ns + "/imu0_config"); + ros::param::del(ns + "/imu0_differential"); + ros::param::del(ns + "/imu0_queue_size"); + ros::param::del(ns + "/imu0_remove_gravitational_acceleration"); + } + + // Diagnostic configuration + ros::param::set(ns + "/diagnostic_updater/frequency", 1.0); + ros::param::set(ns + "/diagnostic_updater/min_frequency", 0.1); + ros::param::set(ns + "/diagnostic_updater/max_frequency", 100.0); +} + +void PIDControllerNode::twistCallback(const geometry_msgs::Twist::ConstPtr& msg) { + // Get the current angular velocity from the Odom + cmd_vel_requested_ = *msg; + last_twist_time_ = ros::Time::now(); +} + +void PIDControllerNode::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) +{ + double angular_velocity = msg->twist.twist.angular.z; + double linear_velocity = msg->twist.twist.linear.x; // Assuming x is the forward direction + + // Apply noise threshold + if (std::abs(angular_velocity) < 0.01) angular_velocity = 0; + if (std::abs(linear_velocity) < 0.01) linear_velocity = 0; + + if (!last_odom_time_.isZero()) { // Check if not the first message + dt_ = (msg->header.stamp - last_odom_time_).toSec(); + } + last_odom_time_ = msg->header.stamp; // Update last_odom_time_ for the next call + + // Calculate the error + double angular_error = cmd_vel_requested_.angular.z - angular_velocity; + double linear_error = cmd_vel_requested_.linear.x - linear_velocity; + + // Reset the integrator if the command should be zero + angular_velocity_pid_.resetIfZero(cmd_vel_requested_.angular.z); + linear_velocity_pid_.resetIfZero(cmd_vel_requested_.linear.x); + + // Update the PID controller + + cmd_vel_.angular.z = cmd_vel_requested_.angular.z; + cmd_vel_.linear.x = cmd_vel_requested_.linear.x; + + if(!pure_feed_forward_) + { + cmd_vel_.angular.z += angular_velocity_pid_.update(angular_error, dt_); + cmd_vel_.linear.x += linear_velocity_pid_.update(linear_error, dt_); + } + + // Publish the control output + cmd_vel_pub_.publish(cmd_vel_); +} + +void PIDControllerNode::checkTimeout() +{ + + if (ros::Time::now() - last_twist_time_ > ros::Duration(odom_timeout_)) { + // No new message within 0.2 seconds, setting cmd_vel_requested_ to zero + cmd_vel_requested_ = geometry_msgs::Twist(); + } + + if (ros::Time::now() - last_odom_time_ > ros::Duration(odom_timeout_)) + { + if (!pure_feed_forward_) + { + pure_feed_forward_ = true; + ROS_WARN("Odometry data timeout, switching to feedforward control."); + } + executeFeedforwardControl(); + } +} + +void PIDControllerNode::executeFeedforwardControl() +{ + cmd_vel_.angular.z = cmd_vel_requested_.angular.z; + cmd_vel_.linear.x = cmd_vel_requested_.linear.x; + + // Publish the control output + cmd_vel_pub_.publish(cmd_vel_); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "PIDControllerNode"); + PIDControllerNode node; + node.run(); + return 0; +} diff --git a/smb_control/src/ekf_restarter.py b/smb_control/src/ekf_restarter.py new file mode 100755 index 0000000..a9d2be4 --- /dev/null +++ b/smb_control/src/ekf_restarter.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 + +import rospy +import rosparam +import subprocess +import time + +class EKFParameterMonitor: + def __init__(self): + rospy.init_node('ekf_parameter_monitor') + + self.namespace = "/ekf_se" + self.params_to_monitor = [ + "odom_frame", "base_link_frame", "world_frame", + "two_d_mode", "frequency", "publish_tf", "print_diagnostics", + "odom0", "odom0_config", "odom0_queue_size", "odom0_differential", "odom0_relative", + "imu0", "imu0_config", "imu0_differential", "imu0_queue_size", "imu0_remove_gravitational_acceleration", + "odom1", "odom1_config", "odom1_queue_size", "odom1_differential", "odom1_relative", + "diagnostic_updater/frequency", "diagnostic_updater/min_frequency", "diagnostic_updater/max_frequency" + ] + + self.previous_params = self.get_current_params() + self.check_interval = rospy.Duration(1.0) # Check every second + + self.timer = rospy.Timer(self.check_interval, self.check_params) + + def get_current_params(self): + current_params = {} + for param in self.params_to_monitor: + full_param = self.namespace + "/" + param + if rospy.has_param(full_param): + current_params[param] = rospy.get_param(full_param) + return current_params + + def check_params(self, event): + current_params = self.get_current_params() + + if current_params != self.previous_params: + rospy.loginfo("Parameters changed, restarting EKF node.") + self.restart_ekf_node() + self.previous_params = current_params + + def restart_ekf_node(self): + # Shutdown the EKF node + subprocess.call(["rosnode", "kill", "/ekf_se"]) + + # Allow some time for the node to shut down + time.sleep(1) + + # Relaunch the EKF node + subprocess.Popen(["roslaunch", "smb_control", "start_ekf_reconfigure.launch"]) + + # Wait for the node to initialize + time.sleep(2) + +if __name__ == "__main__": + try: + monitor = EKFParameterMonitor() + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/smb_gazebo/launch/sim.launch b/smb_gazebo/launch/sim.launch index 1ea49f9..69b14b2 100644 --- a/smb_gazebo/launch/sim.launch +++ b/smb_gazebo/launch/sim.launch @@ -4,7 +4,6 @@ - @@ -20,7 +19,6 @@ - From 7adaf37a9fc55809d7368152a45a229a8a665e68 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 18 Jun 2024 17:57:07 +0200 Subject: [PATCH 2/3] PR comments --- smb_control/src/PIDControllerNode.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/smb_control/src/PIDControllerNode.cpp b/smb_control/src/PIDControllerNode.cpp index d3154a7..903290a 100644 --- a/smb_control/src/PIDControllerNode.cpp +++ b/smb_control/src/PIDControllerNode.cpp @@ -30,7 +30,7 @@ PIDControllerNode::~PIDControllerNode() void PIDControllerNode::run() { - ros::Rate loop_rate(100); // 10 Hz + ros::Rate loop_rate(100); // 100 Hz while (ros::ok()) { ros::spinOnce(); @@ -195,7 +195,6 @@ void PIDControllerNode::checkTimeout() { if (ros::Time::now() - last_twist_time_ > ros::Duration(odom_timeout_)) { - // No new message within 0.2 seconds, setting cmd_vel_requested_ to zero cmd_vel_requested_ = geometry_msgs::Twist(); } From 71e4952908a6b8506768330c9e3f6f533ef4d045 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 18 Jun 2024 18:09:28 +0200 Subject: [PATCH 3/3] twist and odom timeout mixed up --- smb_control/include/smb_control/PIDControllerNode.hpp | 4 ++-- smb_control/src/PIDControllerNode.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/smb_control/include/smb_control/PIDControllerNode.hpp b/smb_control/include/smb_control/PIDControllerNode.hpp index 90c43aa..963bccc 100644 --- a/smb_control/include/smb_control/PIDControllerNode.hpp +++ b/smb_control/include/smb_control/PIDControllerNode.hpp @@ -104,8 +104,8 @@ class PIDControllerNode ros::Time last_twist_time_; ros::Time last_odom_time_; double dt_{0.01}; - double odom_timeout_{0.5}; - double twist_timeout_{0.5}; + double odom_timeout_{0.0}; + double twist_timeout_{0.0}; void checkTimeout(); void executeFeedforwardControl(); diff --git a/smb_control/src/PIDControllerNode.cpp b/smb_control/src/PIDControllerNode.cpp index 903290a..cbea4a4 100644 --- a/smb_control/src/PIDControllerNode.cpp +++ b/smb_control/src/PIDControllerNode.cpp @@ -9,8 +9,8 @@ PIDControllerNode::PIDControllerNode() linear_velocity_pid_(1.5, 1.0, 0.0), config_server_(std::make_shared>(nh_)), - odom_timeout_(0.1), // Timeout of 0.1 seconds - twist_timeout_(0.1) // Timeout of 0.1 seconds + odom_timeout_(0.2), + twist_timeout_(0.1) { // Initialize the publisher cmd_vel_pub_ = nh_.advertise("/control/smb_diff_drive/cmd_vel", 1); @@ -194,7 +194,7 @@ void PIDControllerNode::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) void PIDControllerNode::checkTimeout() { - if (ros::Time::now() - last_twist_time_ > ros::Duration(odom_timeout_)) { + if (ros::Time::now() - last_twist_time_ > ros::Duration(twist_timeout_)) { cmd_vel_requested_ = geometry_msgs::Twist(); }