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

@@ -19,6 +19,7 @@ private:
   ros::NodeHandle nh_;
   ros::Time lastUpdate_;
   controller_manager::ControllerManager controller_;
+  bool robot_state_received_;
 
   // state interfaces
   JointInterface joint_interface_;

src/ros/controller.cpp

@@ -3,6 +3,7 @@
 ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
                              std::vector<std::string>& 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;