get frequency from robot

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_positions_{ 0, 0, 0, 0, 0, 0 }
-  , joint_velocities_{ 0, 0, 0, 0, 0, 0 }
-  , joint_efforts_{ 0, 0, 0, 0, 0, 0 }
+  : joint_position_command_({ 0, 0, 0, 0, 0, 0 })
+  , joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
+  , joint_velocities_{ { 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_efforts_ = { 0, 0, 0, 0, 0, 0 };
+  joint_velocities_ = { { 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;