Added HardwareInterface

src/ros/hardware_interface.cpp

@@ -0,0 +1,129 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-11
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ur_rtde_driver/ros/hardware_interface.h"
+
+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_names_(6)
+
+{
+}
+
+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 };
+  std::string ROBOT_IP = robot_hw_nh.param<std::string>("robot_ip", "192.168.56.101");
+
+  ROS_INFO_STREAM("Initializing urdriver");
+  ur_driver_.reset(new UrDriver(ROBOT_IP));
+
+  if (!root_nh.getParam("hardware_interface/joints", joint_names_))
+  {
+    ROS_ERROR_STREAM("Cannot find required parameter " << root_nh.resolveName("hardware_interface/joints")
+                                                       << " on the parameter server.");
+    throw std::runtime_error("Cannot find required parameter "
+                             "'controller_joint_names' on the parameter server.");
+  }
+
+  // Create ros_control interfaces
+  for (std::size_t i = 0; i < joint_positions_.size(); ++i)
+  {
+    // Create joint state interface for all joints
+    js_interface_.registerHandle(hardware_interface::JointStateHandle(joint_names_[i], &joint_positions_[i],
+                                                                      &joint_velocities_[i], &joint_efforts_[i]));
+
+    // Create joint position control interface
+    pj_interface_.registerHandle(
+        hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
+  }
+
+  // Register interfaces
+  registerInterface(&js_interface_);
+  registerInterface(&pj_interface_);
+
+  ROS_INFO_STREAM_NAMED("hardware_interface", "Loaded ur_rtde_driver hardware_interface");
+
+  return true;
+}
+
+void HardwareInterface ::read(const ros::Time& time, const ros::Duration& period)
+{
+  std::unique_ptr<rtde_interface::DataPackage> data_pkg = ur_driver_->getDataPackage();
+  if (data_pkg)
+  {
+    if (!data_pkg->getData("actual_q", joint_positions_))
+    {
+      // This throwing should never happen unless misconfigured
+      throw std::runtime_error("Did not find joint position in data sent from robot. This should not happen!");
+    }
+    if (!data_pkg->getData("actual_qd", joint_velocities_))
+    {
+      // This throwing should never happen unless misconfigured
+      throw std::runtime_error("Did not find joint velocity in data sent from robot. This should not happen!");
+    }
+  }
+  else
+  {
+    ROS_ERROR("Could not get fresh data package from robot");
+  }
+}
+
+void HardwareInterface ::write(const ros::Time& time, const ros::Duration& period)
+{
+  // TODO: Implement
+}
+
+bool HardwareInterface ::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                                       const std::list<hardware_interface::ControllerInfo>& stop_list)
+{
+  // TODO: Implement
+  return true;
+}
+
+void HardwareInterface ::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                                  const std::list<hardware_interface::ControllerInfo>& stop_list)
+{
+  // TODO: Implement
+}
+
+const uint32_t HardwareInterface ::getControlFrequency() const
+{
+  if (ur_driver_ != nullptr)
+  {
+    return ur_driver_->getControlFrequency();
+  }
+  // TODO: Do this nicer than throwing an exception
+  throw std::runtime_error("ur_driver is not yet initialized");
+}
+}  // namespace ur_driver

src/ros/hardware_interface_node.cpp

@@ -0,0 +1,86 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2019 FZI Forschungszentrum Informatik
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Felix Mauch mauch@fzi.de
+ * \date    2019-04-11
+ *
+ */
+//----------------------------------------------------------------------
+#include <ros/ros.h>
+#include <controller_manager/controller_manager.h>
+
+#include <ur_rtde_driver/ros/hardware_interface.h>
+
+int main(int argc, char** argv)
+{
+  // Set up ROS.
+  ros::init(argc, argv, "ur_hardware_interface");
+  ros::AsyncSpinner spinner(2);
+  spinner.start();
+
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_priv("~");
+
+  ur_driver::HardwareInterface hw_interface;
+
+  // Set up timers
+  ros::Time timestamp;
+  ros::Duration period;
+  auto stopwatch_last = std::chrono::steady_clock::now();
+  auto stopwatch_now = stopwatch_last;
+
+  hw_interface.init(nh, nh_priv);
+  controller_manager::ControllerManager cm(&hw_interface, nh);
+
+  // Get current time and elapsed time since last read
+  timestamp = ros::Time::now();
+  stopwatch_now = std::chrono::steady_clock::now();
+  period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
+  stopwatch_last = stopwatch_now;
+
+  ros::Rate control_rate(static_cast<double>(hw_interface.getControlFrequency()));
+
+  // Run as fast as possible
+  while (ros::ok())
+  {
+    // Receive current state from robot
+    hw_interface.read(timestamp, period);
+
+    // Get current time and elapsed time since last read
+    timestamp = ros::Time::now();
+    stopwatch_now = std::chrono::steady_clock::now();
+    period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
+    stopwatch_last = stopwatch_now;
+
+    cm.update(timestamp, period);
+
+    hw_interface.write(timestamp, period);
+
+    if (!control_rate.sleep())
+    {
+      ROS_WARN_STREAM("Could not keep cycle rate of " << control_rate.expectedCycleTime().toNSec() / 1000000.0 << "ms");
+    }
+  }
+
+  spinner.stop();
+  ROS_INFO_STREAM_NAMED("hardware_interface", "Shutting down.");
+  return 0;
+}

src/ur/ur_driver.cpp

@@ -30,16 +30,23 @@
 
 namespace ur_driver
 {
-ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP)
+ur_driver::UrDriver::UrDriver(const std::string& ROBOT_IP) : rtde_frequency_(125)  // conservative CB3 default.
 {
+  ROS_INFO_STREAM("Initializing RTDE client");
   rtde_client_.reset(new rtde_interface::RTDEClient(ROBOT_IP, notifier_));
+
+  if (!rtde_client_->init())
+  {
+    throw std::runtime_error("initialization went wrong");  // TODO: be less harsh
+  }
+  rtde_client_->start();  // TODO: Add extra start method (also to HW-Interface)
 }
 
 std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage()
 {
   // TODO: This goes into the rtde_client
   std::unique_ptr<comm::URPackage<rtde_interface::PackageHeader>> urpackage;
-  uint32_t period_ms = (1.0 / rtde_frequency_) / 1000;
+  uint32_t period_ms = (1.0 / rtde_frequency_) * 1000;
   std::chrono::milliseconds timeout(period_ms);
   if (rtde_client_->getDataPackage(urpackage, timeout))
   {