moved driver to subfolder

ur_rtde_driver/src/ros/hardware_interface.cpp

@@ -0,0 +1,152 @@
+// 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)
+  , position_controller_running_(false)
+
+{
+}
+
+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]));
+  }
+
+  speedsc_interface_.registerHandle(
+      ur_controllers::SpeedScalingHandle("speed_scaling_factor", &speed_scaling_combined_));
+
+  // Register interfaces
+  registerInterface(&js_interface_);
+  registerInterface(&pj_interface_);
+  registerInterface(&speedsc_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!");
+    }
+    if (!data_pkg->getData("target_speed_fraction", speed_scaling_value_))
+    {
+      // This throwing should never happen unless misconfigured
+      throw std::runtime_error("Did not find speed_scaling 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)
+{
+  if (position_controller_running_)
+  {
+    ur_driver_->writeJointCommand(joint_position_command_);
+  }
+}
+
+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)
+{
+  position_controller_running_ = false;
+  for (auto& controller_it : start_list)
+  {
+    for (auto& resource_it : controller_it.claimed_resources)
+    {
+      if (resource_it.hardware_interface == "hardware_interface::PositionJointInterface")
+      {
+        position_controller_running_ = true;
+      }
+    }
+  }
+}
+
+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

ur_rtde_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;
+}

ur_rtde_driver/src/ros/service_stopper.cpp

@@ -0,0 +1,97 @@
+/*
+ * Copyright 2017, 2018 Simon Rasmussen (refactor)
+ *
+ * Copyright 2015, 2016 Thomas Timm Andersen (original version)
+ *
+ * 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.
+ */
+
+#include "ur_rtde_driver/ros/service_stopper.h"
+
+namespace ur_driver
+{
+ServiceStopper::ServiceStopper(std::vector<Service*> services)
+  : enable_service_(nh_.advertiseService("ur_driver/robot_enable", &ServiceStopper::enableCallback, this))
+  , services_(services)
+  , last_state_(RobotState::Error)
+  , activation_mode_(ActivationMode::Never)
+{
+  std::string mode;
+  ros::param::param("~require_activation", mode, std::string("Never"));
+  if (mode == "Always")
+  {
+    activation_mode_ = ActivationMode::Always;
+  }
+  else if (mode == "OnStartup")
+  {
+    activation_mode_ = ActivationMode::OnStartup;
+  }
+  else
+  {
+    if (mode != "Never")
+    {
+      LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always",
+               mode.c_str());
+      mode = "Never";
+    }
+    notify_all(RobotState::Running);
+  }
+
+  LOG_INFO("Service 'ur_driver/robot_enable' activation mode: %s", mode.c_str());
+}
+
+bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
+{
+  // After the startup call OnStartup and Never behave the same
+  if (activation_mode_ == ActivationMode::OnStartup)
+    activation_mode_ = ActivationMode::Never;
+  notify_all(RobotState::Running);
+  return true;
+}
+
+void ServiceStopper::notify_all(RobotState state)
+{
+  if (last_state_ == state)
+    return;
+
+  for (auto const service : services_)
+  {
+    service->onRobotStateChange(state);
+  }
+
+  last_state_ = state;
+}
+
+bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
+{
+  if (data.emergency_stopped)
+  {
+    notify_all(RobotState::EmergencyStopped);
+  }
+  else if (data.protective_stopped)
+  {
+    notify_all(RobotState::ProtectiveStopped);
+  }
+  else if (error)
+  {
+    notify_all(RobotState::Error);
+  }
+  else if (activation_mode_ == ActivationMode::Never)
+  {
+    // No error encountered, the user requested automatic reactivation
+    notify_all(RobotState::Running);
+  }
+
+  return true;
+}
+}  // namespace ur_driver