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;