From af601b9c3291450a60c77593b5433d41d8a18dcb Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 10:27:29 +0200 Subject: [PATCH 1/6] Implemented ros-control --- CMakeLists.txt | 18 ++- config/ur10_controllers.yaml | 55 ++++++++ config/ur5_controllers.yaml | 60 ++++++++ include/ur_modern_driver/robot_state_RT.h | 8 +- .../ur_modern_driver/ur_hardware_interface.h | 117 +++++++++++++++ launch/ur10_ros_control.launch | 40 ++++++ launch/ur5_bringup.launch | 2 +- launch/ur5_ros_control.launch | 40 ++++++ package.xml | 6 + src/robot_state_RT.cpp | 25 +++- src/ur_hardware_interface.cpp | 133 ++++++++++++++++++ src/ur_ros_wrapper.cpp | 74 ++++++++-- 12 files changed, 549 insertions(+), 29 deletions(-) create mode 100644 config/ur10_controllers.yaml create mode 100644 config/ur5_controllers.yaml create mode 100644 include/ur_modern_driver/ur_hardware_interface.h create mode 100644 launch/ur10_ros_control.launch create mode 100644 launch/ur5_ros_control.launch create mode 100644 src/ur_hardware_interface.cpp 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/config/ur10_controllers.yaml b/config/ur10_controllers.yaml new file mode 100644 index 0000000..1389f4d --- /dev/null +++ b/config/ur10_controllers.yaml @@ -0,0 +1,55 @@ +# 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: + # joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + # joint2: {p: 2.0, i: 0.0, d: 0.01, 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..0f629ea --- /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: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, 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/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 77823f1..ae90b71 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -56,7 +56,8 @@ private: std::mutex val_lock_; // Locks the variables while unpack parses data; std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool new_data_available_; //to avoid spurious wakes + bool data_published_; //to avoid spurious wakes + bool controller_updated_; //to avoid spurious wakes std::vector unpackVector(uint8_t * buf, int start_index, int nr_of_vals); @@ -98,7 +99,10 @@ public: void setVersion(double ver); - void finishedReading(); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); std::vector getVActual(); void unpack(uint8_t * buf); }; 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..707ad53 --- /dev/null +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -0,0 +1,117 @@ +/* + * 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(); + +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_; + + // 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..ca35ba3 --- /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/robot_state_RT.cpp b/src/robot_state_RT.cpp index 602f310..273db0e 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -41,23 +41,33 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { v_robot_ = 0.0; i_robot_ = 0.0; v_actual_.assign(6, 0.0); - new_data_available_ = false; + data_published_ = false; + controller_updated_ = false; pMsg_cond_ = &msg_cond; } RobotStateRT::~RobotStateRT() { /* Make sure nobody is waiting after this thread is destroyed */ - new_data_available_ = true; + data_published_ = true; + controller_updated_ = true; pMsg_cond_->notify_all(); } -bool RobotStateRT::getNewDataAvailable() { - return new_data_available_; + +void RobotStateRT::setDataPublished() { + data_published_ = false; +} +bool RobotStateRT::getDataPublished() { + return data_published_; } -void RobotStateRT::finishedReading() { - new_data_available_ = false; +void RobotStateRT::setControllerUpdated() { + controller_updated_ = false; } +bool RobotStateRT::getControllerUpdated() { + return controller_updated_; +} + double RobotStateRT::ntohd(uint64_t nf) { double x; @@ -394,7 +404,8 @@ void RobotStateRT::unpack(uint8_t * buf) { v_actual_ = unpackVector(buf, offset, 6); } val_lock_.unlock(); - new_data_available_ = true; + controller_updated_ = true; + data_published_ = true; pMsg_cond_->notify_all(); } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp new file mode 100644 index 0000000..9705af4 --- /dev/null +++ b/src/ur_hardware_interface.cpp @@ -0,0 +1,133 @@ +/* + * 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. +} + +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() { + 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); +} + +} // namespace diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index e67c017..08edc72 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,10 +129,12 @@ public: payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); - rt_publish_thread_ = new std::thread( + 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)); + 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,18 +343,49 @@ 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(); + + // Control + //controller_manager_->update(ros::Time::now(), elapsed_time); + controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time); + + // Output + hardware_interface_->write(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + } + } + 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(); geometry_msgs::WrenchStamped wrench_msg; 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_->getNewDataAvailable()) { + while (!robot_.rt_interface_->robot_state_->getDataPublished()) { rt_msg_cond_.wait(locker); } joint_msg.header.stamp = ros::Time::now(); @@ -365,13 +409,13 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); - robot_.rt_interface_->robot_state_->finishedReading(); + robot_.rt_interface_->robot_state_->setDataPublished(); } } 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 +487,11 @@ int main(int argc, char **argv) { RosWrapper interface(host); - ros::spin(); + ros::AsyncSpinner spinner(3); + spinner.start(); + + ros::waitForShutdown(); + interface.halt(); exit(0); From 1e8f13f2323f2c8f2608b75a763ba906671275dd Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 13:50:53 +0200 Subject: [PATCH 2/6] Added control switching --- README.md | 2 - config/ur10_controllers.yaml | 25 ++++++---- config/ur5_controllers.yaml | 12 ++--- .../ur_modern_driver/ur_hardware_interface.h | 4 +- launch/ur5_ros_control.launch | 4 +- src/ur_hardware_interface.cpp | 47 ++++++++++++++++++- src/ur_ros_wrapper.cpp | 4 +- 7 files changed, 73 insertions(+), 25 deletions(-) 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 index 1389f4d..54c62be 100644 --- a/config/ur10_controllers.yaml +++ b/config/ur10_controllers.yaml @@ -33,7 +33,7 @@ position_trajectory_controller: - wrist_1_joint - wrist_2_joint - wrist_3_joint - constraints: + constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} @@ -42,14 +42,19 @@ position_trajectory_controller: 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: - # joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - # joint2: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 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 + # 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 index 0f629ea..54c62be 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -47,12 +47,12 @@ position_trajectory_controller: action_monitor_rate: 10 gains: #!!These values have not been optimized!! - shoulder_pan_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - shoulder_lift_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - elbow_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - wrist_1_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - wrist_2_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} - wrist_3_joint: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + 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 diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index 707ad53..5720811 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -86,6 +86,8 @@ public: /// \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 @@ -95,7 +97,7 @@ protected: 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_; diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index ca35ba3..050553b 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -28,8 +28,8 @@ - + diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 9705af4..e5cf596 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -106,6 +106,7 @@ void UrHardwareInterface::init() { 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() { @@ -127,7 +128,51 @@ void UrHardwareInterface::read() { } void UrHardwareInterface::write() { - 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); + 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 08edc72..3c0d382 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -363,14 +363,12 @@ private: last_time = current_time; // Input hardware_interface_->read(); - + robot_.rt_interface_->robot_state_->setControllerUpdated(); // Control - //controller_manager_->update(ros::Time::now(), elapsed_time); controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time); // Output hardware_interface_->write(); - robot_.rt_interface_->robot_state_->setControllerUpdated(); } } From 639597eacdb2738a7be6255b274942dea115b896 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 14:11:07 +0200 Subject: [PATCH 3/6] Re-included io_states publisher --- src/ur_ros_wrapper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 3c0d382..e92f9e0 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -132,9 +132,9 @@ public: ros_control_thread_ = new std::thread( boost::bind(&RosWrapper::rosControlLoop, this)); /*rt_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishRTMsg, this)); + boost::bind(&RosWrapper::publishRTMsg, this)); */ mb_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishMbMsg, this)); */ + boost::bind(&RosWrapper::publishMbMsg, this)); ROS_DEBUG("The action server for this driver has been started"); } From 8ae3a86a625b8952e9b973153e310d8108d2e90f Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 14:18:24 +0200 Subject: [PATCH 4/6] Clean up --- include/ur_modern_driver/robot_state_RT.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index ae90b71..bec0767 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -95,7 +95,6 @@ public: double getVMain(); double getVRobot(); double getIRobot(); - bool getNewDataAvailable(); void setVersion(double ver); From 039664a1a31c79935f4b45eb7694ccff1e0c06e3 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 15:47:47 +0200 Subject: [PATCH 5/6] Added reconnect to RT port --- .../ur_realtime_communication.h | 2 + src/ur_realtime_communication.cpp | 100 +++++++++++++++--- 2 files changed, 90 insertions(+), 12 deletions(-) diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index 2e43e43..2a8be1f 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -30,6 +30,8 @@ #include #include #include +#include +#include #ifdef ROS_BUILD #include diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index 23ecfd2..ebb2e6e 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -42,6 +42,7 @@ UrRealtimeCommunication::UrRealtimeCommunication( flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); connected_ = false; keepalive_ = false; safety_count_ = safety_count_max + 1; @@ -49,18 +50,30 @@ UrRealtimeCommunication::UrRealtimeCommunication( } void UrRealtimeCommunication::start() { + fd_set writefds; + struct timeval timeout; + keepalive_ = true; #ifdef ROS_BUILD ROS_DEBUG("Realtime port: Connecting..."); #else printf("Realtime port: Connecting...\n"); #endif - if (connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)) - < 0) { + + connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + timeout.tv_sec = 10; + timeout.tv_usec = 0; + select(sockfd_ + 1, NULL, &writefds, NULL, &timeout); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { #ifdef ROS_BUILD - ROS_FATAL("Error connecting to RT port 30003"); + ROS_FATAL("Error connecting to RT port 30003 - errno: %d (%s)", flag_, strerror(flag_)); #else - printf("Error connecting to RT port 30003\n"); + printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_, + strerror(flag_)); #endif } sockaddr_in name; @@ -70,7 +83,8 @@ void UrRealtimeCommunication::start() { #ifdef ROS_BUILD ROS_FATAL("Could not get local IP - errno: %d (%s)", errno, strerror(errno)); #else - printf("Could not get local IP - errno: %d (%s)", errno, strerror(errno)); + printf("Could not get local IP - errno: %d (%s)", errno, + strerror(errno)); #endif } char str[18]; @@ -109,6 +123,10 @@ void UrRealtimeCommunication::run() { uint8_t buf[2048]; int bytes_read; bzero(buf, 2048); + struct timeval timeout; + fd_set readfds; + FD_ZERO(&readfds); + FD_SET(sockfd_, &readfds); #ifdef ROS_BUILD ROS_DEBUG("Realtime port: Got connection"); #else @@ -116,14 +134,72 @@ void UrRealtimeCommunication::run() { #endif connected_ = true; while (keepalive_) { - bytes_read = read(sockfd_, buf, 2048); - setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, - sizeof(int)); - robot_state_->unpack(buf); - if (safety_count_ == safety_count_max_) { - setSpeed(0., 0., 0., 0., 0., 0.); + 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(sockfd_ + 1, &readfds, NULL, NULL, &timeout); + bytes_read = read(sockfd_, buf, 2048); + if (bytes_read > 0) { + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + sizeof(int)); + robot_state_->unpack(buf); + if (safety_count_ == safety_count_max_) { + setSpeed(0., 0., 0., 0., 0., 0.); + } + safety_count_ += 1; + } else { + connected_ = false; + close(sockfd_); + } + } + if (keepalive_) { + //reconnect +#ifdef ROS_BUILD + ROS_WARN("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); +#else + printf( + "Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds...\n"); +#endif + sockfd_ = socket(AF_INET, SOCK_STREAM, 0); + if (sockfd_ < 0) { +#ifdef ROS_BUILD + ROS_FATAL("ERROR opening socket"); + ros::shutdown(); +#else + printf("ERROR opening socket"); + exit(1); +#endif + } + flag_ = 1; + setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, + sizeof(int)); + setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (char *) &flag_, + sizeof(int)); + fcntl(sockfd_, F_SETFL, O_NONBLOCK); + while (keepalive_ && !connected_) { + std::this_thread::sleep_for(std::chrono::seconds(10)); + fd_set writefds; + keepalive_ = true; + + connect(sockfd_, (struct sockaddr *) &serv_addr_, + sizeof(serv_addr_)); + FD_ZERO(&writefds); + FD_SET(sockfd_, &writefds); + select(sockfd_ + 1, NULL, &writefds, NULL, NULL); + unsigned int flag_len; + getsockopt(sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); + if (flag_ < 0) { +#ifdef ROS_BUILD + ROS_ERROR("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); +#else + printf( + "Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); +#endif + } else { + connected_ = true; + } + } } - safety_count_ += 1; } setSpeed(0., 0., 0., 0., 0., 0.); close(sockfd_); From 6e9f646b02d55b8f7e32c1447cf40a78a7a6efc2 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 24 Sep 2015 16:10:33 +0200 Subject: [PATCH 6/6] Added reconnect to secondary port --- include/ur_modern_driver/ur_communication.h | 4 +- src/ur_communication.cpp | 104 +++++++++++++++++--- src/ur_driver.cpp | 23 ++--- 3 files changed, 107 insertions(+), 24 deletions(-) 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/src/ur_communication.cpp b/src/ur_communication.cpp index 5b2a27f..502cb6f 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"); ros::shutdown(); + return false; #else printf("Error connecting\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() {