get frequency from robot

ur_controllers/include/ur_controllers/hardware_interface_adapter.h

@@ -15,7 +15,6 @@
 #ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
 #define UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
 
-
 #include <joint_trajectory_controller/hardware_interface_adapter.h>
 #include "ur_controllers/scaled_joint_command_interface.h"
 
@@ -43,20 +42,24 @@ template <class State>
 class HardwareInterfaceAdapter<ur_controllers::ScaledPositionJointInterface, State>
 {
 public:
-  HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
+  HardwareInterfaceAdapter() : joint_handles_ptr_(0)
+  {
+  }
 
   bool init(std::vector<ur_controllers::ScaledJointHandle>& joint_handles, ros::NodeHandle& /*controller_nh*/)
   {
     // Store pointer to joint handles
     joint_handles_ptr_ = &joint_handles;
 
-
     return true;
   }
 
   void starting(const ros::Time& /*time*/)
   {
-    if (!joint_handles_ptr_) {return;}
+    if (!joint_handles_ptr_)
+    {
+      return;
+    }
 
     // Semantic zero for commands
     for (auto& jh : *joint_handles_ptr_)
@@ -65,20 +68,23 @@ public:
     }
   }
 
-  void stopping(const ros::Time& /*time*/) {}
+  void stopping(const ros::Time& /*time*/)
+  {
+  }
 
-  void updateCommand(const ros::Time&     /*time*/,
+  void updateCommand(const ros::Time& /*time*/, const ros::Duration& /*period*/, const State& desired_state,
-                     const ros::Duration& /*period*/,
+                     const State& /*state_error*/)
-                     const State&         desired_state,
-                     const State&         /*state_error*/)
   {
     // Forward desired position to command
     const unsigned int n_joints = joint_handles_ptr_->size();
-    for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
+    for (unsigned int i = 0; i < n_joints; ++i)
+    {
+      (*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);
+    }
   }
 
 private:
   std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
 };
 
-#endif // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED
+#endif  // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED

ur_controllers/include/ur_controllers/speed_scaling_state_controller.h

@@ -36,7 +36,7 @@ public:
 
 private:
   std::vector<SpeedScalingHandle> sensors_;
-  //TODO: We should use a better datatype later on
+  // TODO: We should use a better datatype later on
   typedef std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64> > RtPublisherPtr;
   std::vector<RtPublisherPtr> realtime_pubs_;
   std::vector<ros::Time> last_publish_times_;

ur_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -18,7 +18,6 @@
 #include <pluginlib/class_list_macros.hpp>
 #include <trajectory_interface/quintic_spline_segment.h>
 
-
 namespace position_controllers
 {
 typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,

ur_controllers/src/speed_scaling_state_controller.cpp

@@ -86,4 +86,3 @@ void SpeedScalingStateController ::stopping(const ros::Time& /*time*/)
 }  // namespace ur_controllers
 
 PLUGINLIB_EXPORT_CLASS(ur_controllers::SpeedScalingStateController, controller_interface::ControllerBase)
-

ur_rtde_driver/include/ur_rtde_driver/rtde/rtde_client.h

@@ -57,12 +57,19 @@ public:
   bool start();
   bool getDataPackage(std::unique_ptr<comm::URPackage<PackageHeader>>& data_package, std::chrono::milliseconds timeout);
 
+  double getMaxFrequency() const
+  {
+    return max_frequency_;
+  }
+
 private:
   comm::URStream<PackageHeader> stream_;
   RTDEParser parser_;
   comm::URProducer<PackageHeader> prod_;
   comm::Pipeline<PackageHeader> pipeline_;
 
+  double max_frequency_;
+
   constexpr static const double CB3_MAX_FREQUENCY = 125.0;
   constexpr static const double URE_MAX_FREQUENCY = 500.0;
 

ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h

@@ -56,7 +56,7 @@ public:
    */
   std::unique_ptr<rtde_interface::DataPackage> getDataPackage();
 
-  const uint32_t& getControlFrequency() const
+  uint32_t getControlFrequency() const
   {
     return rtde_frequency_;
   }
@@ -64,8 +64,7 @@ public:
   bool writeJointCommand(const vector6d_t& values);
 
 private:
-  //! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series.
+  int rtde_frequency_;
-  uint32_t rtde_frequency_;
   comm::INotifier notifier_;
   std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
   std::unique_ptr<comm::ReverseInterface> reverse_interface_;

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -30,10 +30,10 @@
 namespace ur_driver
 {
 HardwareInterface::HardwareInterface()
-  : joint_position_command_{ 0, 0, 0, 0, 0, 0 }
+  : joint_position_command_({ 0, 0, 0, 0, 0, 0 })
-  , joint_positions_{ 0, 0, 0, 0, 0, 0 }
+  , joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
-  , joint_velocities_{ 0, 0, 0, 0, 0, 0 }
+  , joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
-  , joint_efforts_{ 0, 0, 0, 0, 0, 0 }
+  , joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
   , joint_names_(6)
   , position_controller_running_(false)
 
@@ -42,8 +42,8 @@ HardwareInterface::HardwareInterface()
 
 bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
 {
-  joint_velocities_ = { 0, 0, 0, 0, 0, 0 };
+  joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } };
-  joint_efforts_ = { 0, 0, 0, 0, 0, 0 };
+  joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } };
   std::string ROBOT_IP = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
 
   ROS_INFO_STREAM("Initializing urdriver");

ur_rtde_driver/src/ros/hardware_interface_node.cpp

@@ -48,7 +48,9 @@ int main(int argc, char** argv)
   auto stopwatch_now = stopwatch_last;
 
   hw_interface.init(nh, nh_priv);
+  ROS_INFO_STREAM("initialized hw interface");
   controller_manager::ControllerManager cm(&hw_interface, nh);
+  ROS_INFO_STREAM("started controller manager");
 
   // Get current time and elapsed time since last read
   timestamp = ros::Time::now();
@@ -76,7 +78,8 @@ int main(int argc, char** argv)
 
     if (!control_rate.sleep())
     {
-      ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << "ms");
+      // ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 <<
+      // "ms");
     }
   }
 

ur_rtde_driver/src/rtde/rtde_client.cpp

@@ -36,6 +36,7 @@ RTDEClient::RTDEClient(std::string ROBOT_IP, comm::INotifier& notifier)
   , parser_(readRecipe())
   , prod_(stream_, parser_)
   , pipeline_(prod_, PIPELINE_NAME, notifier)
+  , max_frequency_(URE_MAX_FREQUENCY)
 {
 }
 
@@ -68,7 +69,6 @@ bool RTDEClient::init()
   }
 
   // determine maximum frequency from ur-control version
-  double max_frequency = URE_MAX_FREQUENCY;
   size = GetUrcontrolVersionRequest::generateSerializedRequest(buffer);
   stream_.write(buffer, size, written);
   pipeline_.getLatestProduct(package, std::chrono::milliseconds(1000));
@@ -76,14 +76,14 @@ bool RTDEClient::init()
       dynamic_cast<rtde_interface::GetUrcontrolVersion*>(package.get());
   if (tmp_control_version->major_ < 5)
   {
-    max_frequency = CB3_MAX_FREQUENCY;
+    max_frequency_ = CB3_MAX_FREQUENCY;
   }
 
   // sending output recipe
-  LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency);
+  LOG_INFO("Setting up RTDE communication with frequency %f", max_frequency_);
   if (protocol_version == 2)
   {
-    size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency, readRecipe());
+    size = ControlPackageSetupOutputsRequest::generateSerializedRequest(buffer, max_frequency_, readRecipe());
   }
   else
   {

ur_rtde_driver/src/ur/ur_driver.cpp

@@ -62,7 +62,7 @@ def myProg():
         stopj(1.0)
         sync()
       elif state == SERVO_RUNNING:
-        servoj(q, t=0.008, lookahead_time=0.03, gain=300)
+        servoj(q, t=0.008, lookahead_time=0.03, gain=750)
       else:
         sync()
       end
@@ -90,7 +90,7 @@ def myProg():
 end
 )";
 
-ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125)  // conservative CB3 default.
+ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP)
 {
   ROS_INFO_STREAM("Initializing RTDE client");
   rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
@@ -100,6 +100,8 @@ ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125
     throw std::runtime_error("initialization went wrong");  // TODO: be less harsh
   }
 
+  rtde_frequency_ = rtde_client_->getMaxFrequency();
+
   comm::URStream<rtde_interface::PackageHeader> stream(ROBOT_IP, 30001);
   stream.connect();
 
@@ -119,7 +121,10 @@ ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125
   uint32_t reverse_port = 50001;  // TODO: Make this a parameter
   reverse_interface_.reset(new comm::ReverseInterface(reverse_port));
 
+  ROS_INFO_STREAM("Created reverse interface");
+
   rtde_client_->start();  // TODO: Add extra start method (also to HW-Interface)
+  ROS_INFO_STREAM("Initialization done");
 }
 
 std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
@@ -134,7 +139,7 @@ std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage
     if (tmp != nullptr)
     {
       urpackage.release();
-      return std::move(std::unique_ptr<rtde_interface::DataPackage>(tmp));
+      return std::unique_ptr<rtde_interface::DataPackage>(tmp);
     }
   }
   return nullptr;