diff --git a/CMakeLists.txt b/CMakeLists.txt index fe9844e..aebf6a2 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/ur_rtde_driver/ros/hardware_interface.h b/include/ur_rtde_driver/ros/hardware_interface.h new file mode 100644 index 0000000..d0d9678 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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& start_list, + const std::list& stop_list) override; + virtual void doSwitch(const std::list& start_list, + const std::list& stop_list) override; + + const uint32_t getControlFrequency() const; + +protected: + std::unique_ptr ur_driver_; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::PositionJointInterface pj_interface_; + // hardware_interface::VelocityJointInterface vj_interface_; + + vector6d_t joint_position_command_; + // std::vector joint_velocity_command_; + vector6d_t joint_positions_; + vector6d_t joint_velocities_; + vector6d_t joint_efforts_; + std::vector joint_names_; +}; + +} // namespace ur_driver + +#endif // ifndef UR_DRIVER_HARDWARE_INTERFACE_H_INCLUDED diff --git a/include/ur_rtde_driver/ur/ur_driver.h b/include/ur_rtde_driver/ur/ur_driver.h index 90a1b28..33a4bf7 100644 --- a/include/ur_rtde_driver/ur/ur_driver.h +++ b/include/ur_rtde_driver/ur/ur_driver.h @@ -55,6 +55,11 @@ public: */ std::unique_ptr 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_; diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch index 1baf399..4c6a5b4 100644 --- a/launch/ur10_ros_control.launch +++ b/launch/ur10_ros_control.launch @@ -16,8 +16,8 @@ - - + + @@ -25,18 +25,18 @@ - - - - - - - - - - - - + + + + + + + + + + + + @@ -47,8 +47,8 @@ output="screen" args="$(arg controllers)" /> - + + diff --git a/src/ros/hardware_interface.cpp b/src/ros/hardware_interface.cpp new file mode 100644 index 0000000..27f81f6 --- /dev/null +++ b/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("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 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& start_list, + const std::list& stop_list) +{ + // TODO: Implement + return true; +} + +void HardwareInterface ::doSwitch(const std::list& start_list, + const std::list& 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 diff --git a/src/ros/hardware_interface_node.cpp b/src/ros/hardware_interface_node.cpp new file mode 100644 index 0000000..d8dfa63 --- /dev/null +++ b/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 +#include + +#include + +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>(stopwatch_now - stopwatch_last).count()); + stopwatch_last = stopwatch_now; + + ros::Rate control_rate(static_cast(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>(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; +} diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e701b3a..d85dbcd 100644 --- a/src/ur/ur_driver.cpp +++ b/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 ur_driver::UrDriver::getDataPackage() { // TODO: This goes into the rtde_client std::unique_ptr> 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)) {