From 2f1251c2eb350d82a3f027c5ced19a6d91a20cfb Mon Sep 17 00:00:00 2001 From: Miguel Prada Date: Mon, 5 Nov 2018 14:05:30 +0100 Subject: [PATCH] Resolve premature controller initialization (#213) Squashed commits: * Avoid updating ros_control controllers on pipeline timeout * Revert "Avoid updating ros_control controllers on pipeline timeout" * Do not update controller_manager until RTPackets have been received * Mark handles as initialized after actually doing it rather than just before * Improve readability --- include/ur_modern_driver/ros/controller.h | 1 + src/ros/controller.cpp | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/include/ur_modern_driver/ros/controller.h b/include/ur_modern_driver/ros/controller.h index df5d6de..6aa781a 100644 --- a/include/ur_modern_driver/ros/controller.h +++ b/include/ur_modern_driver/ros/controller.h @@ -19,6 +19,7 @@ private: ros::NodeHandle nh_; ros::Time lastUpdate_; controller_manager::ControllerManager controller_; + bool robot_state_received_; // state interfaces JointInterface joint_interface_; diff --git a/src/ros/controller.cpp b/src/ros/controller.cpp index bc844c4..b73f437 100644 --- a/src/ros/controller.cpp +++ b/src/ros/controller.cpp @@ -3,6 +3,7 @@ ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector& joint_names, double max_vel_change, std::string tcp_link) : controller_(this, nh_) + , robot_state_received_(false) , joint_interface_(joint_names) , wrench_interface_(tcp_link) , position_interface_(follower, joint_interface_, joint_names) @@ -81,10 +82,15 @@ void ROSController::read(RTShared& packet) { joint_interface_.update(packet); wrench_interface_.update(packet); + robot_state_received_ = true; } bool ROSController::update() { + // don't run controllers if we haven't received robot state yet + if (!robot_state_received_) + return true; + auto time = ros::Time::now(); auto diff = time - lastUpdate_; lastUpdate_ = time;