Added HardwareInterface

CMakeLists.txt

@@ -97,18 +97,26 @@ add_library(ur_rtde_driver
 target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES})
 add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_executable(ur_rtde_driver_node
+  src/ros/hardware_interface.cpp
+  src/ros/hardware_interface_node.cpp
+)
+target_link_libraries(ur_rtde_driver_node ${catkin_LIBRARIES} ur_rtde_driver)
+add_dependencies(ur_rtde_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 add_executable(plain_driver
   src/main_plain_driver.cpp
 )
 target_link_libraries(plain_driver ${catkin_LIBRARIES} ur_rtde_driver)
 add_dependencies(plain_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
-install(TARGETS ur_rtde_driver
+install(TARGETS ur_rtde_driver ur_rtde_driver_node
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+
 install(DIRECTORY config launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 

include/ur_rtde_driver/ros/hardware_interface.h

@@ -0,0 +1,72 @@
+// 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
+ *
+ */
+//----------------------------------------------------------------------
+#ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
+#define UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED
+
+#include <hardware_interface/robot_hw.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/joint_state_interface.h>
+#include <algorithm>
+
+#include "ur_rtde_driver/ur/ur_driver.h"
+
+namespace ur_driver
+{
+class HardwareInterface : public hardware_interface::RobotHW
+{
+public:
+  HardwareInterface();
+  virtual ~HardwareInterface() = default;
+  virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh) override;
+  virtual void read(const ros::Time& time, const ros::Duration& period) override;
+  virtual void write(const ros::Time& time, const ros::Duration& period) override;
+  virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                             const std::list<hardware_interface::ControllerInfo>& stop_list) override;
+  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
+                        const std::list<hardware_interface::ControllerInfo>& stop_list) override;
+
+  const uint32_t getControlFrequency() const;
+
+protected:
+  std::unique_ptr<UrDriver> ur_driver_;
+
+  hardware_interface::JointStateInterface js_interface_;
+  hardware_interface::PositionJointInterface pj_interface_;
+  // hardware_interface::VelocityJointInterface vj_interface_;
+
+  vector6d_t joint_position_command_;
+  // std::vector<double> joint_velocity_command_;
+  vector6d_t joint_positions_;
+  vector6d_t joint_velocities_;
+  vector6d_t joint_efforts_;
+  std::vector<std::string> joint_names_;
+};
+
+}  // namespace ur_driver
+
+#endif  // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED

include/ur_rtde_driver/ur/ur_driver.h

@@ -55,6 +55,11 @@ public:
    */
   std::unique_ptr<rtde_interface::DataPackage> getDataPackage();
 
+  const uint32_t& getControlFrequency() const
+  {
+    return rtde_frequency_;
+  }
+
 private:
   //! This frequency is used for the rtde interface. By default, it will be 125Hz on CB3 and 500Hz on the e-series.
   uint32_t rtde_frequency_;

launch/ur10_ros_control.launch

@@ -16,8 +16,8 @@
   <arg name="base_frame" default="$(arg prefix)base" />
   <arg name="tool_frame" default="$(arg prefix)tool0_controller" />
   <arg name="shutdown_on_disconnect" default="true" />
-  <arg name="controllers" default="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller"/>
-  <arg name="stopped_controllers" default="pos_based_pos_traj_controller joint_group_vel_controller"/>
+  <arg name="controllers" default="joint_state_controller"/>
+  <arg name="stopped_controllers" default=""/>
   <!-- robot model -->
   <include file="$(find ur_description)/launch/ur10_upload.launch">
     <arg name="limited" value="$(arg limited)"/>
@@ -25,18 +25,18 @@
 
 
   <!-- Load hardware interface -->
-  <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_driver" output="log" launch-prefix="$(arg launch_prefix)">
-    <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
-    <param name="reverse_port" type="int" value="$(arg reverse_port)" />
-    <param name="min_payload" type="double" value="$(arg min_payload)"/>
-    <param name="max_payload" type="double" value="$(arg max_payload)"/>
-    <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
-    <param name="use_ros_control" type="bool" value="True"/>
-    <param name="servoj_gain" type="double" value="750" />
-    <param name="prefix" value="$(arg prefix)" />
-    <param name="base_frame" type="str" value="$(arg base_frame)"/>
-    <param name="tool_frame" type="str" value="$(arg tool_frame)"/>
-    <param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>
+  <node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)">
+    <param name="robot_ip" type="str" value="$(arg robot_ip)"/>
+    <!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->
+    <!--<param name="min_payload" type="double" value="$(arg min_payload)"/>-->
+    <!--<param name="max_payload" type="double" value="$(arg max_payload)"/>-->
+    <!--<param name="max_velocity" type="double" value="$(arg max_velocity)"/>-->
+    <!--<param name="use_ros_control" type="bool" value="True"/>-->
+    <!--<param name="servoj_gain" type="double" value="750" />-->
+    <!--<param name="prefix" value="$(arg prefix)" />-->
+    <!--<param name="base_frame" type="str" value="$(arg base_frame)"/>-->
+    <!--<param name="tool_frame" type="str" value="$(arg tool_frame)"/>-->
+    <!--<param name="shutdown_on_disconnect" type="bool" value="$(arg shutdown_on_disconnect)"/>-->
   </node>
 
   <!-- Load controller settings -->
@@ -47,8 +47,8 @@
     output="screen" args="$(arg controllers)" />
 
   <!-- load other controller -->
-  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-    output="screen" args="load $(arg stopped_controllers)" />
+  <!--<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"-->
+    <!--output="screen" args="load $(arg stopped_controllers)" />-->
 
   <!-- Convert joint states to /tf tranforms -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

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))
   {