diff --git a/CMakeLists.txt b/CMakeLists.txt index f77beb0..078d776 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,8 @@ add_definitions( -DROS_BUILD ) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + hardware_interface + controller_manager actionlib control_msgs geometry_msgs @@ -107,8 +109,8 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include # LIBRARIES ur_modern_driver -# CATKIN_DEPENDS actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs -# DEPENDS system_lib + CATKIN_DEPENDS hardware_interface controller_manager actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs + DEPENDS ur_hardware_interface ) ########### @@ -135,9 +137,12 @@ include_directories(include ) ## Declare a C++ library -# add_library(ur_modern_driver -# src/${PROJECT_NAME}/ur_modern_driver.cpp -# ) + +# Hardware Interface +add_library(ur_hardware_interface src/ur_hardware_interface.cpp) +target_link_libraries(ur_hardware_interface + ${catkin_LIBRARIES} +) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -160,7 +165,8 @@ add_executable(ur_driver ${${PROJECT_NAME}_SOURCES}) ## Specify libraries to link a library or executable target against target_link_libraries(ur_driver - ${catkin_LIBRARIES} + ur_hardware_interface + ${catkin_LIBRARIES} ) ############# diff --git a/README.md b/README.md index 7e6ad23..136e208 100644 --- a/README.md +++ b/README.md @@ -41,8 +41,6 @@ Besides this, the driver subscribes to two new topics: */joint\_speed : takes messages of type trajectory\_msgs/JointTrajectory, parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. -This driver is written in c++, which should make it possible to integrate it with ros_control. If you feel like undertaking this task, please dive right in. I don't have the posibility to do this. - No script is sent to the robot. This means that the teach pendant can be used to move the robot around while the driver is running. --- diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml new file mode 100644 index 0000000..54c62be --- /dev/null +++ b/config/ur10_controllers.yaml @@ -0,0 +1,60 @@ +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_trajectory_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml new file mode 100644 index 0000000..54c62be --- /dev/null +++ b/config/ur5_controllers.yaml @@ -0,0 +1,60 @@ +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_trajectory_controller: + type: velocity_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + elbow_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 50.0, i: 0.0, d: 0.1, i_clamp: 1} + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #hold_trajectory_duration: 0 # Defaults to 0.5 + diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index c9bc361..f9c1b45 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -29,6 +29,8 @@ #include #include #include +#include +#include #ifdef ROS_BUILD #include @@ -49,7 +51,7 @@ public: RobotState* robot_state_; UrCommunication(std::condition_variable& msg_cond, std::string host); - void start(); + bool start(); void halt(); }; diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h new file mode 100644 index 0000000..5720811 --- /dev/null +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -0,0 +1,119 @@ +/* + * ur_hardware_control_loop.cpp + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * wrote this file. As long as you retain this notice you + * can do whatever you want with this stuff. If we meet some day, and you think + * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen + * ---------------------------------------------------------------------------- + */ + +/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: Example ros_control hardware interface that performs a perfect control loop for simulation + */ + +#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H +#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H + +#include +#include +#include +#include +#include +#include +#include +#include "ur_driver.h" + +namespace ros_control_ur { + +// For simulation only - determines how fast a trajectory is followed +static const double POSITION_STEP_FACTOR = 1; +static const double VELOCITY_STEP_FACTOR = 1; + +/// \brief Hardware interface for a robot +class UrHardwareInterface: public hardware_interface::RobotHW { +public: + + /** + * \brief Constructor + * \param nh - Node handle for topics. + */ + UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); + + /// \brief Initialize the hardware interface + virtual void init(); + + /// \brief Read the state from the robot hardware. + virtual void read(); + + /// \brief write the command to the robot hardware. + virtual void write(); + + void doSwitch(const std::list&, const std::list&); + +protected: + + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; + + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + bool velocity_interface_running_; + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = {0.,0.,0.}; + double robot_torque_[3] = {0.,0.,0.}; + + // Robot API + UrDriver* robot_; + +}; +// class + +}// namespace + +#endif diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch new file mode 100644 index 0000000..733ccfe --- /dev/null +++ b/launch/ur10_ros_control.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index fca474b..212b8e6 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -12,7 +12,7 @@ - + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch new file mode 100644 index 0000000..050553b --- /dev/null +++ b/launch/ur5_ros_control.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 73ac34b..24faa14 100644 --- a/package.xml +++ b/package.xml @@ -40,6 +40,8 @@ catkin + hardware_interface + controller_manager actionlib control_msgs geometry_msgs @@ -48,6 +50,9 @@ std_msgs trajectory_msgs ur_msgs + hardware_interface + controller_manager + ros_controllers actionlib control_msgs geometry_msgs @@ -56,6 +61,7 @@ std_msgs trajectory_msgs ur_msgs + ur_description diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 5b406ec..00ba198 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -61,11 +61,12 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond, sizeof(int)); setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); connected_ = false; keepalive_ = false; } -void UrCommunication::start() { +bool UrCommunication::start() { keepalive_ = true; uint8_t buf[512]; unsigned int bytes_read; @@ -81,9 +82,10 @@ void UrCommunication::start() { #ifdef ROS_BUILD ROS_FATAL("Error connecting to get firmware version"); ros::shutdown(); + return false; #else printf("Error connecting to get firmware version\n"); - exit(1); + return false; #endif } #ifdef ROS_BUILD @@ -110,14 +112,27 @@ void UrCommunication::start() { printf( "Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic #endif - if (connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, - sizeof(sec_serv_addr_)) < 0) { + + fd_set writefds; + struct timeval timeout; + + connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sec_sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { #ifdef ROS_BUILD - ROS_FATAL("Error connecting"); + ROS_FATAL("Error connecting to secondary interface"); ros::shutdown(); + return false; #else - printf("Error connecting\n"); - exit(1); + printf("Error connecting to secondary interface\n"); + return false; #endif } #ifdef ROS_BUILD @@ -126,6 +141,7 @@ void UrCommunication::start() { printf("Secondary interface: Got connection\n"); #endif comThread_ = std::thread(&UrCommunication::run, this); + return true; } void UrCommunication::halt() { @@ -135,15 +151,79 @@ void UrCommunication::halt() { void UrCommunication::run() { uint8_t buf[2048]; - unsigned int bytes_read; + int bytes_read; bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sec_sockfd_, &readfds); connected_ = true; while (keepalive_) { - bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes - setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf, bytes_read); + while (connected_ && keepalive_) { + timeout.tv_sec = 0; //do this each loop as selects modifies timeout + timeout.tv_usec = 500000; // timeout of 0.5 sec + select(sec_sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sec_sockfd_, buf, 2048); // usually only up to 1295 bytes + if (bytes_read > 0) { + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, + (char *) &flag_, sizeof(int)); + robot_state_->unpack(buf, bytes_read); + } else { + connected_ = false; + close (sec_sockfd_); + } + } + if (keepalive_) { + //reconnect +#ifdef ROS_BUILD + ROS_WARN("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); +#else + printf( + "Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n"); +#endif + sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sec_sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening secondary socket"); + ros::shutdown(); +#else + printf("ERROR opening secondary socket"); + exit(1); +#endif + } + flag_ = 1; + setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + sizeof(int)); + setsockopt(sec_sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, + sizeof(int)); + fcntl(sec_sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; + keepalive_ = true; + + connect(sec_sockfd_, (struct sockaddr *) &sec_serv_addr_, + sizeof(sec_serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sec_sockfd_, &writefds); + select(sec_sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, + &flag_len); + if (flag_ < 0) { +#ifdef ROS_BUILD + ROS_ERROR("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); +#else + printf( + "Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); +#endif + } else { + connected_ = true; + } + } + } } + //wait for some traffic so the UR socket doesn't die in version 3.1. std::this_thread::sleep_for(std::chrono::milliseconds(500)); close(sec_sockfd_); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 382f7ec..9a7bdd6 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -91,9 +91,9 @@ void UrDriver::addTraj(std::vector inp_timestamps, } //make sure we come to a smooth stop /*while (timestamps.back() < inp_timestamps.back()) { - timestamps.push_back(timestamps.back() + 0.008); - } - timestamps.pop_back();*/ + timestamps.push_back(timestamps.back() + 0.008); + } + timestamps.pop_back();*/ unsigned int j = 0; for (unsigned int i = 0; i < timestamps.size(); i++) { @@ -162,7 +162,7 @@ void UrDriver::doTraj(std::vector inp_timestamps, } positions = UrDriver::interp_cubic( std::chrono::duration_cast>( - t - t0).count()- inp_timestamps[j - 1], + t - t0).count() - inp_timestamps[j - 1], inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); for (int i = 0; i < 6; i++) { @@ -297,16 +297,17 @@ void UrDriver::uploadProg() { } void UrDriver::start() { - sec_interface_->start(); - rt_interface_->robot_state_->setVersion( - sec_interface_->robot_state_->getVersion()); - rt_interface_->start(); - ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); + if (sec_interface_->start()) { + rt_interface_->robot_state_->setVersion( + sec_interface_->robot_state_->getVersion()); + rt_interface_->start(); + ip_addr_ = rt_interface_->getLocalIp(); //inet_ntoa(serv_addr.sin_addr); #ifdef ROS_BUILD - ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); + ROS_DEBUG("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); #else - printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); + printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); #endif + } } void UrDriver::halt() { diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp new file mode 100644 index 0000000..e5cf596 --- /dev/null +++ b/src/ur_hardware_interface.cpp @@ -0,0 +1,178 @@ +/* + * ur_hardware_control_loop.cpp + * + * ---------------------------------------------------------------------------- + * "THE BEER-WARE LICENSE" (Revision 42): + * wrote this file. As long as you retain this notice you + * can do whatever you want with this stuff. If we meet some day, and you think + * this stuff is worth it, you can buy me a beer in return. Thomas Timm Andersen + * ---------------------------------------------------------------------------- + */ + +/* Based on original source from University of Colorado, Boulder. License copied below. Feel free to offer Dave a beer as well if you meet him. */ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: Example ros_control hardware interface that performs a perfect control loop for simulation + */ + +#include + +namespace ros_control_ur { + +UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : + nh_(nh), robot_(robot) { + // Initialize shared memory and interfaces here + init(); // this implementation loads from rosparam + + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); +} + +void UrHardwareInterface::init() { + ROS_INFO_STREAM_NAMED("ur_hardware_interface", + "Reading rosparams from namespace: " << nh_.getNamespace()); + + // Get joint names + nh_.getParam("hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) { + ROS_FATAL_STREAM_NAMED("ur_hardware_interface", + "No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace()); + exit(-1); + } + num_joints_ = joint_names_.size(); + + // Resize vectors + joint_position_.resize(num_joints_); + joint_velocity_.resize(num_joints_); + joint_effort_.resize(num_joints_); + joint_velocity_command_.resize(num_joints_); + + // Initialize controller + for (std::size_t i = 0; i < num_joints_; ++i) { + ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", + "Loading joint name: " << joint_names_[i]); + + // Create joint state interface + joint_state_interface_.registerHandle( + hardware_interface::JointStateHandle(joint_names_[i], + &joint_position_[i], &joint_velocity_[i], + &joint_effort_[i])); + + // Create velocity joint interface + velocity_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_velocity_command_[i])); + } + + // Create joint state interface + force_torque_interface_.registerHandle( + hardware_interface::ForceTorqueSensorHandle("wrench", "", + robot_force_, robot_torque_)); + + registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&velocity_joint_interface_); // From RobotHW base class. + registerInterface(&force_torque_interface_); // From RobotHW base class. + velocity_interface_running_ = false; +} + +void UrHardwareInterface::read() { + std::vector pos, vel, current, tcp; + pos = robot_->rt_interface_->robot_state_->getQActual(); + vel = robot_->rt_interface_->robot_state_->getQdActual(); + current = robot_->rt_interface_->robot_state_->getIActual(); + tcp = robot_->rt_interface_->robot_state_->getTcpForce(); + for (std::size_t i = 0; i < num_joints_; ++i) { + joint_position_[i] = pos[i]; + joint_velocity_[i] = vel[i]; + joint_effort_[i] = current[i]; + } + for (std::size_t i = 0; i < 3; ++i) { + robot_force_[i] = tcp[i]; + robot_torque_[i] = tcp[i + 3]; + } + +} + +void UrHardwareInterface::write() { + if (velocity_interface_running_) { + robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1], + joint_velocity_command_[2], joint_velocity_command_[3], + joint_velocity_command_[4], joint_velocity_command_[5], 100); + } +} + +void UrHardwareInterface::doSwitch( + const std::list& start_list, + const std::list& stop_list) { + for (std::list::const_iterator controller_it = + start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = true; + ROS_DEBUG("Starting velocity interface"); + } + } + for (std::list::const_iterator controller_it = + stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + if (controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + velocity_interface_running_ = false; + ROS_DEBUG("Stopping velocity interface"); + } + } + /*std::string outp; + outp = "doSwitch called - Start list:"; + for (std::list::const_iterator controller_it = + start_list.begin(); controller_it != start_list.end(); + ++controller_it) { + outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")"; + } + outp += " - Stop list:"; + for (std::list::const_iterator controller_it = + stop_list.begin(); controller_it != stop_list.end(); + ++controller_it) { + outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")"; + + } + ROS_INFO(outp.c_str()); */ + +} + +} // namespace + diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 732e4f6..e92f9e0 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -10,6 +10,7 @@ */ #include "ur_modern_driver/ur_driver.h" +#include "ur_modern_driver/ur_hardware_interface.h" #include #include #include @@ -17,6 +18,7 @@ #include #include #include +#include #include "ros/ros.h" #include @@ -35,6 +37,7 @@ #include "ur_msgs/Digital.h" #include "ur_msgs/Analog.h" #include "std_msgs/String.h" +#include class RosWrapper { protected: @@ -52,9 +55,12 @@ protected: ros::ServiceServer payload_srv_; std::thread* rt_publish_thread_; std::thread* mb_publish_thread_; + std::thread* ros_control_thread_; double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; + boost::shared_ptr hardware_interface_; + boost::shared_ptr controller_manager_; public: RosWrapper(std::string host) : @@ -76,6 +82,11 @@ public: joint_names.push_back(joint_prefix + "wrist_3_joint"); robot_.setJointNames(joint_names); + hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_)); + controller_manager_.reset( + new controller_manager::ControllerManager( + hardware_interface_.get(), nh_)); + //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! max_velocity_ = 10.; if (ros::param::get("~max_velocity", max_velocity_)) { @@ -102,13 +113,13 @@ public: robot_.start(); //register the goal and feedback callbacks - as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this)); - as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this)); + //as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this)); + //as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this)); - as_.start(); + //as_.start(); //subscribe to the data topic of interest - speed_sub_ = nh_.subscribe("joint_speed", 1, + speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this); urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this); @@ -118,8 +129,10 @@ public: payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); - rt_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishRTMsg, this)); + ros_control_thread_ = new std::thread( + boost::bind(&RosWrapper::rosControlLoop, this)); + /*rt_publish_thread_ = new std::thread( + boost::bind(&RosWrapper::publishRTMsg, this)); */ mb_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishMbMsg, this)); ROS_DEBUG("The action server for this driver has been started"); @@ -269,7 +282,7 @@ private: traj.points[i].velocities[mapping[j]]); if (traj.points[i].accelerations.size() != 0) new_point.accelerations.push_back( - traj.points[i].accelerations[mapping[j]]); + traj.points[i].accelerations[mapping[j]]); } new_point.time_from_start = traj.points[i].time_from_start; new_traj.push_back(new_point); @@ -330,11 +343,40 @@ private: } + void rosControlLoop() { + ros::Duration elapsed_time; + struct timespec last_time, current_time; + static const double BILLION = 1000000000.0; + + clock_gettime(CLOCK_MONOTONIC, &last_time); + while (ros::ok()) { + std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex + std::unique_lock locker(msg_lock); + while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) { + rt_msg_cond_.wait(locker); + } + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + elapsed_time = ros::Duration( + current_time.tv_sec - last_time.tv_sec + + (current_time.tv_nsec - last_time.tv_nsec) + / BILLION); + last_time = current_time; + // Input + hardware_interface_->read(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + // Control + controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time); + + // Output + hardware_interface_->write(); + } + } + void publishRTMsg() { ros::Publisher joint_pub = nh_.advertise( - "/joint_states", 1); + "joint_states", 1); ros::Publisher wrench_pub = nh_.advertise( - "/wrench", 1); + "wrench", 1); while (ros::ok()) { sensor_msgs::JointState joint_msg; joint_msg.name = robot_.getJointNames(); @@ -371,7 +413,7 @@ private: } void publishMbMsg() { - ros::Publisher io_pub = nh_.advertise("/io_states", + ros::Publisher io_pub = nh_.advertise("ur_driver/io_states", 1); while (ros::ok()) { @@ -443,7 +485,11 @@ int main(int argc, char **argv) { RosWrapper interface(host); - ros::spin(); + ros::AsyncSpinner spinner(3); + spinner.start(); + + ros::waitForShutdown(); + interface.halt(); exit(0);