diff --git a/CMakeLists.txt b/CMakeLists.txt index 078d776..1a9702e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -156,7 +156,8 @@ set(${PROJECT_NAME}_SOURCES src/ur_realtime_communication.cpp src/ur_communication.cpp src/robot_state.cpp - src/robot_state_RT.cpp) + src/robot_state_RT.cpp + src/do_output.cpp) add_executable(ur_driver ${${PROJECT_NAME}_SOURCES}) ## Add cmake target dependencies of the executable diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml index dd679bc..57591ca 100644 --- a/config/ur5_controllers.yaml +++ b/config/ur5_controllers.yaml @@ -22,7 +22,43 @@ force_torque_sensor_controller: type: force_torque_sensor_controller/ForceTorqueSensorController publish_rate: 125 -# Joint Trajectory Controller ------------------------------- +# Joint Trajectory Controller - position based ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_based_position_trajectory_controller: + type: position_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 + +# Joint Trajectory Controller - velocity based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller velocity_based_position_trajectory_controller: type: velocity_controllers/JointTrajectoryController diff --git a/include/ur_modern_driver/do_output.h b/include/ur_modern_driver/do_output.h new file mode 100644 index 0000000..8031821 --- /dev/null +++ b/include/ur_modern_driver/do_output.h @@ -0,0 +1,27 @@ +/* + * do_output.h + * + * ---------------------------------------------------------------------------- + * "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 + * ---------------------------------------------------------------------------- + */ + +#ifndef UR_DO_OUTPUT_H_ +#define UR_DO_OUTPUT_H_ + +#ifdef ROS_BUILD +#include +#endif +#include + +void print_debug(std::string inp); +void print_info(std::string inp); +void print_warning(std::string inp); +void print_error(std::string inp); +void print_fatal(std::string inp); + + +#endif /* UR_DO_OUTPUT_H_ */ diff --git a/include/ur_modern_driver/ur_communication.h b/include/ur_modern_driver/ur_communication.h index f9c1b45..7adb165 100644 --- a/include/ur_modern_driver/ur_communication.h +++ b/include/ur_modern_driver/ur_communication.h @@ -13,6 +13,7 @@ #define UR_COMMUNICATION_H_ #include "robot_state.h" +#include "do_output.h" #include #include #include @@ -32,9 +33,6 @@ #include #include -#ifdef ROS_BUILD -#include -#endif class UrCommunication { private: diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 4a162b4..fc98214 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -16,6 +16,7 @@ #include #include "ur_realtime_communication.h" #include "ur_communication.h" +#include "do_output.h" #include #include #include @@ -65,7 +66,7 @@ public: void uploadProg(); void openServo(); - void closeServo(); + void closeServo(std::vector positions); std::vector interp_cubic(double t, double T, std::vector p0_pos, std::vector p1_pos, diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h index f06932c..c2de2d7 100644 --- a/include/ur_modern_driver/ur_hardware_interface.h +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -9,7 +9,7 @@ * ---------------------------------------------------------------------------- */ -/* 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. */ +/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */ /********************************************************************* * Software License Agreement (BSD License) @@ -58,6 +58,7 @@ #include #include #include +#include "do_output.h" #include "ur_driver.h" namespace ros_control_ur { @@ -85,7 +86,11 @@ public: /// \brief write the command to the robot hardware. virtual void write(); - void doSwitch(const std::list&, const std::list&); + bool canSwitch( + const std::list &start_list, + const std::list &stop_list) const; + void doSwitch(const std::list&start_list, + const std::list&stop_list); protected: @@ -95,17 +100,20 @@ protected: // Interfaces hardware_interface::JointStateInterface joint_state_interface_; hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::PositionJointInterface position_joint_interface_; hardware_interface::VelocityJointInterface velocity_joint_interface_; bool velocity_interface_running_; + bool position_interface_running_; // Shared memory std::vector joint_names_; std::vector joint_position_; std::vector joint_velocity_; std::vector joint_effort_; + std::vector joint_position_command_; std::vector joint_velocity_command_; std::size_t num_joints_; - double robot_force_[3] = {0.,0.,0.}; - double robot_torque_[3] = {0.,0.,0.}; + double robot_force_[3] = { 0., 0., 0. }; + double robot_torque_[3] = { 0., 0., 0. }; // Robot API UrDriver* robot_; diff --git a/include/ur_modern_driver/ur_realtime_communication.h b/include/ur_modern_driver/ur_realtime_communication.h index f415172..66b5a4b 100644 --- a/include/ur_modern_driver/ur_realtime_communication.h +++ b/include/ur_modern_driver/ur_realtime_communication.h @@ -13,6 +13,7 @@ #define UR_REALTIME_COMMUNICATION_H_ #include "robot_state_RT.h" +#include "do_output.h" #include #include #include @@ -33,10 +34,6 @@ #include #include -#ifdef ROS_BUILD -#include -#endif - class UrRealtimeCommunication { private: unsigned int safety_count_max_; diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch index c08d6c0..b9644b7 100644 --- a/launch/ur5_ros_control.launch +++ b/launch/ur5_ros_control.launch @@ -30,8 +30,8 @@ - + diff --git a/src/do_output.cpp b/src/do_output.cpp new file mode 100644 index 0000000..2327231 --- /dev/null +++ b/src/do_output.cpp @@ -0,0 +1,50 @@ +/* + * do_output.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 + * ---------------------------------------------------------------------------- + */ + +#include "ur_modern_driver/du_output.h" + +void print_debug(std::string inp) { +#ifdef ROS_BUILD + ROS_DEBUG(inp); +#else + printf("DEBUG: %s\n", inp.c_str()); +#endif +} +void print_info(std::string inp) { +#ifdef ROS_BUILD + ROS_INFO(inp); +#else + printf("INFO: %s\n", inp.c_str()); +#endif +} +void print_warning(std::string inp) { +#ifdef ROS_BUILD + ROS_WARN(inp); +#else + printf("WARNING: %s\n", inp.c_str()); +#endif +} +void print_error(std::string inp) { +#ifdef ROS_BUILD + ROS_ERROR(inp); +#else + printf("ERROR: %s\n", inp.c_str()); +#endif +} +void print_fatal(std::string inp) { +#ifdef ROS_BUILD + ROS_FATAL(inp); + ros::shutdown(); +#else + printf("FATAL: %s\n", inp.c_str()); + exit(1); +#endif +} diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp index 00ba198..3b81892 100644 --- a/src/ur_communication.cpp +++ b/src/ur_communication.cpp @@ -18,33 +18,15 @@ UrCommunication::UrCommunication(std::condition_variable& msg_cond, bzero((char *) &sec_serv_addr_, sizeof(sec_serv_addr_)); pri_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); if (pri_sockfd_ < 0) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR opening socket"); - ros::shutdown(); -#else - printf("ERROR opening socket"); - exit(1); -#endif + print_fatal("ERROR opening socket pri_sockfd"); } sec_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); if (sec_sockfd_ < 0) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR opening socket"); - ros::shutdown(); -#else - printf("ERROR opening socket"); - exit(1); -#endif + print_fatal("ERROR opening socket sec_sockfd"); } server_ = gethostbyname(host.c_str()); if (server_ == NULL) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR, no such host"); - ros::shutdown(); -#else - printf("ERROR, no such host\n"); - exit(1); -#endif + print_fatal("ERROR, unknown host"); } pri_serv_addr_.sin_family = AF_INET; sec_serv_addr_.sin_family = AF_INET; @@ -72,46 +54,26 @@ bool UrCommunication::start() { unsigned int bytes_read; std::string cmd; bzero(buf, 512); -#ifdef ROS_BUILD - ROS_DEBUG("Acquire firmware version: Connecting..."); -#else - printf("Acquire firmware version: Connecting...\n"); -#endif + print_debug("Acquire firmware version: Connecting..."); if (connect(pri_sockfd_, (struct sockaddr *) &pri_serv_addr_, sizeof(pri_serv_addr_)) < 0) { -#ifdef ROS_BUILD - ROS_FATAL("Error connecting to get firmware version"); - ros::shutdown(); + print_fatal("Error connecting to get firmware version"); return false; -#else - printf("Error connecting to get firmware version\n"); - return false; -#endif } -#ifdef ROS_BUILD - ROS_DEBUG("Acquire firmware version: Got connection"); -#else - printf("Acquire firmware version: Got connection\n"); -#endif + print_debug("Acquire firmware version: Got connection"); bytes_read = read(pri_sockfd_, buf, 512); setsockopt(pri_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, sizeof(int)); robot_state_->unpack(buf, bytes_read); //wait for some traffic so the UR socket doesn't die in version 3.1. std::this_thread::sleep_for(std::chrono::milliseconds(500)); -#ifdef ROS_BUILD - ROS_DEBUG("Firmware version detected: %1.7f", robot_state_->getVersion()); -#else - printf("Firmware version detected: %f\n", robot_state_->getVersion()); -#endif + char tmp[64]; + sprintf(tmp, "Firmware version detected: %.7f", robot_state_->getVersion()); + print_debug(tmp); close(pri_sockfd_); -#ifdef ROS_BUILD - ROS_DEBUG("Switching to secondary interface for masterboard data: Connecting..."); -#else - printf( - "Switching to secondary interface for masterboard data: Connecting...\n"); // which generates less network traffic -#endif + print_debug( + "Switching to secondary interface for masterboard data: Connecting..."); fd_set writefds; struct timeval timeout; @@ -126,20 +88,10 @@ bool UrCommunication::start() { unsigned int flag_len; getsockopt(sec_sockfd_, SOL_SOCKET, SO_ERROR, &flag_, &flag_len); if (flag_ < 0) { -#ifdef ROS_BUILD - ROS_FATAL("Error connecting to secondary interface"); - ros::shutdown(); + print_fatal("Error connecting to secondary interface"); return false; -#else - printf("Error connecting to secondary interface\n"); - return false; -#endif } -#ifdef ROS_BUILD - ROS_DEBUG("Secondary interface: Got connection"); -#else - printf("Secondary interface: Got connection\n"); -#endif + print_debug("Secondary interface: Got connection"); comThread_ = std::thread(&UrCommunication::run, this); return true; } @@ -170,26 +122,15 @@ void UrCommunication::run() { robot_state_->unpack(buf, bytes_read); } else { connected_ = false; - close (sec_sockfd_); + 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 + print_warning("Secondary port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); 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 + print_fatal("ERROR opening secondary socket"); } flag_ = 1; setsockopt(sec_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, @@ -211,12 +152,7 @@ void UrCommunication::run() { 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 + print_error("Error re-connecting to port 30002. Is controller started? Will try to reconnect in 10 seconds..."); } else { connected_ = true; } diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 85515b7..ee144a1 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -24,15 +24,12 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max); + new_sockfd_ = -1; sec_interface_ = new UrCommunication(msg_cond, host); incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0); if (incoming_sockfd_ < 0) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR opening socket for reverse communication"); -#else - printf("ERROR opening socket for reverse communication"); -#endif + print_fatal("ERROR opening socket for reverse communication"); } bzero((char *) &serv_addr, sizeof(serv_addr)); @@ -45,11 +42,7 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond, setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); if (bind(incoming_sockfd_, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR on binding socket for reverse communication"); -#else - printf("ERROR on binding socket for reverse communication"); -#endif + print_fatal("ERROR on binding socket for reverse communication"); } listen(incoming_sockfd_, 5); } @@ -158,9 +151,7 @@ void UrDriver::doTraj(std::vector inp_timestamps, t = std::chrono::high_resolution_clock::now(); } //Signal robot to stop driverProg() - UrDriver::servoj(positions, 0.008, 0); - UrDriver::closeServo(); - + UrDriver::closeServo(positions); } void UrDriver::servoj(std::vector positions, double time, @@ -277,14 +268,14 @@ void UrDriver::openServo() { new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr *) &cli_addr, &clilen); if (new_sockfd_ < 0) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR on accepting reverse communication"); -#else - printf("ERROR on accepting reverse communication"); -#endif + print_fatal("ERROR on accepting reverse communication"); } } -void UrDriver::closeServo() { +void UrDriver::closeServo(std::vector positions) { + if (positions.size() != 6) + UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0.008, 0); + else + UrDriver::servoj(positions, 0.008, 0); close(new_sockfd_); } @@ -296,11 +287,9 @@ bool UrDriver::start() { if (!rt_interface_->start()) return false; 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_); -#else - printf("Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); -#endif + char buf[256]; + sprintf(buf, "Listening on %s:%u\n", ip_addr_.c_str(), REVERSE_PORT_); + print_debug(buf); return true; } @@ -327,62 +316,37 @@ void UrDriver::setJointNames(std::vector jn) { void UrDriver::setToolVoltage(unsigned int v) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); -#ifdef ROS_BUILD - ROS_DEBUG("%s", buf); -#else - printf("%s", buf); -#endif - rt_interface_->addCommandToQueue(buf); + print_debug(buf); } void UrDriver::setFlag(unsigned int n, bool b) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False"); -#ifdef ROS_BUILD - ROS_DEBUG("%s", buf); -#else - printf("%s", buf); -#endif - rt_interface_->addCommandToQueue(buf); - + print_debug(buf); } void UrDriver::setDigitalOut(unsigned int n, bool b) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False"); -#ifdef ROS_BUILD - ROS_DEBUG("%s", buf); -#else - printf("%s", buf); -#endif - rt_interface_->addCommandToQueue(buf); + print_debug(buf); } void UrDriver::setAnalogOut(unsigned int n, double f) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f); -#ifdef ROS_BUILD - ROS_DEBUG("%s", buf); -#else - printf("%s", buf); -#endif - rt_interface_->addCommandToQueue(buf); + print_debug(buf); } bool UrDriver::setPayload(double m) { if ((m < maximum_payload_) && (m > minimum_payload_)) { char buf[256]; sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); -#ifdef ROS_BUILD - ROS_DEBUG("%s", buf); -#else - printf("%s", buf); -#endif rt_interface_->addCommandToQueue(buf); + print_debug(buf); return true; } else return false; diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp index 3cd6732..20860db 100644 --- a/src/ur_hardware_interface.cpp +++ b/src/ur_hardware_interface.cpp @@ -9,7 +9,7 @@ * ---------------------------------------------------------------------------- */ -/* 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. */ +/* Based on original source from University of Colorado, Boulder. License copied below. Please offer Dave a beer as well if you meet him. */ /********************************************************************* * Software License Agreement (BSD License) @@ -45,7 +45,7 @@ * POSSIBILITY OF SUCH DAMAGE. ********************************************************************* - Author: Dave Coleman + Author: Dave Coleman */ #include @@ -77,6 +77,7 @@ void UrHardwareInterface::init() { joint_position_.resize(num_joints_); joint_velocity_.resize(num_joints_); joint_effort_.resize(num_joints_); + joint_position_command_.resize(num_joints_); joint_velocity_command_.resize(num_joints_); // Initialize controller @@ -90,6 +91,12 @@ void UrHardwareInterface::init() { &joint_position_[i], &joint_velocity_[i], &joint_effort_[i])); + // Create position joint interface + position_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_position_command_[i])); + // Create velocity joint interface velocity_joint_interface_.registerHandle( hardware_interface::JointHandle( @@ -97,15 +104,17 @@ void UrHardwareInterface::init() { &joint_velocity_command_[i])); } - // Create joint state interface + // Create force torque interface force_torque_interface_.registerHandle( hardware_interface::ForceTorqueSensorHandle("wrench", "", robot_force_, robot_torque_)); registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&position_joint_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; + position_interface_running_ = false; } void UrHardwareInterface::read() { @@ -131,9 +140,81 @@ 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); + } else if (position_interface_running_) { + robot_->servoj(joint_position_command_, 0.008, 1); } } +bool UrHardwareInterface::canSwitch( + const std::list &start_list, + const std::list &stop_list) const { + 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") { + if (velocity_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (position_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = + stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a PositionJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } else if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + if (position_interface_running_) { + ROS_ERROR( + "%s: An interface of that type (%s) is already running", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + if (velocity_interface_running_) { + bool error = true; + for (std::list::const_iterator stop_controller_it = + stop_list.begin(); + stop_controller_it != stop_list.end(); + ++stop_controller_it) { + if (stop_controller_it->hardware_interface + == "hardware_interface::VelocityJointInterface") { + error = false; + break; + } + } + if (error) { + ROS_ERROR( + "%s (type %s) can not be run simultaneously with a VelocityJointInterface", + controller_it->name.c_str(), + controller_it->hardware_interface.c_str()); + return false; + } + } + } + } +// we can always stop a controller + return true; +} + void UrHardwareInterface::doSwitch( const std::list& start_list, const std::list& stop_list) { @@ -145,6 +226,12 @@ void UrHardwareInterface::doSwitch( velocity_interface_running_ = true; ROS_DEBUG("Starting velocity interface"); } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = true; + robot_->uploadProg(); + ROS_DEBUG("Starting position interface"); + } } for (std::list::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); @@ -154,22 +241,14 @@ void UrHardwareInterface::doSwitch( velocity_interface_running_ = false; ROS_DEBUG("Stopping velocity interface"); } + if (controller_it->hardware_interface + == "hardware_interface::PositionJointInterface") { + position_interface_running_ = false; + std::vector tmp; + robot_->closeServo(tmp); + ROS_DEBUG("Stopping position 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()); */ } diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp index d785e5c..9f12b70 100644 --- a/src/ur_realtime_communication.cpp +++ b/src/ur_realtime_communication.cpp @@ -18,23 +18,11 @@ UrRealtimeCommunication::UrRealtimeCommunication( bzero((char *) &serv_addr_, sizeof(serv_addr_)); 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 + print_fatal("ERROR opening socket"); } server_ = gethostbyname(host.c_str()); if (server_ == NULL) { -#ifdef ROS_BUILD - ROS_FATAL("ERROR, no such host"); - ros::shutdown(); -#else - printf("ERROR, no such host\n"); - exit(1); -#endif + print_fatal("ERROR, no such host"); } serv_addr_.sin_family = AF_INET; bcopy((char *) server_->h_addr, (char *)&serv_addr_.sin_addr.s_addr, server_->h_length); @@ -54,11 +42,7 @@ bool UrRealtimeCommunication::start() { struct timeval timeout; keepalive_ = true; -#ifdef ROS_BUILD - ROS_DEBUG("Realtime port: Connecting..."); -#else - printf("Realtime port: Connecting...\n"); -#endif + print_debug("Realtime port: Connecting..."); connect(sockfd_, (struct sockaddr *) &serv_addr_, sizeof(serv_addr_)); FD_ZERO(&writefds); @@ -69,25 +53,14 @@ bool UrRealtimeCommunication::start() { 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 - errno: %d (%s)", flag_, strerror(flag_)); - -#else - printf("Error connecting to RT port 30003 - errno: %d (%s)\n", flag_, - strerror(flag_)); -#endif + print_fatal("Error connecting to RT port 30003"); return false; } sockaddr_in name; socklen_t namelen = sizeof(name); int err = getsockname(sockfd_, (sockaddr*) &name, &namelen); if (err < 0) { -#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)); -#endif + print_fatal("Could not get local IP"); close(sockfd_); return false; } @@ -132,11 +105,7 @@ void UrRealtimeCommunication::run() { fd_set readfds; FD_ZERO(&readfds); FD_SET(sockfd_, &readfds); -#ifdef ROS_BUILD - ROS_DEBUG("Realtime port: Got connection"); -#else - printf("Realtime port: Got connection\n"); -#endif + print_debug("Realtime port: Got connection"); connected_ = true; while (keepalive_) { while (connected_ && keepalive_) { @@ -159,21 +128,10 @@ void UrRealtimeCommunication::run() { } 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 + print_warning("Realtime port: No connection. Is controller crashed? Will try to reconnect in 10 seconds..."); 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 + print_fatal("ERROR opening socket"); } flag_ = 1; setsockopt(sockfd_, IPPROTO_TCP, TCP_NODELAY, (char *) &flag_, @@ -194,12 +152,7 @@ void UrRealtimeCommunication::run() { 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 + print_error("Error re-connecting to RT port 30003. Is controller started? Will try to reconnect in 10 seconds..."); } else { connected_ = true; } diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 9e448ce..d2066fc 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -11,6 +11,7 @@ #include "ur_modern_driver/ur_driver.h" #include "ur_modern_driver/ur_hardware_interface.h" +#include "ur_modern_driver/do_output.h" #include #include #include @@ -71,9 +72,11 @@ public: std::string joint_prefix = ""; std::vector joint_names; + char buf[256]; if (ros::param::get("~prefix", joint_prefix)) { - ROS_INFO("Setting prefix to %s", joint_prefix.c_str()); + sprintf(buf,"Setting prefix to %s", joint_prefix.c_str()); + print_info(buf); } joint_names.push_back(joint_prefix + "shoulder_pan_joint"); joint_names.push_back(joint_prefix + "shoulder_lift_joint"); @@ -96,8 +99,9 @@ public: //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_)) { - ROS_DEBUG("Max velocity accepted by ur_driver: %f [rad/s]", + sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity_); + print_debug(buf); } //Bounds for SetPayload service @@ -105,21 +109,24 @@ public: double min_payload = 0.; double max_payload = 1.; if (ros::param::get("~min_payload", min_payload)) { - ROS_DEBUG("Min payload set to: %f [kg]", min_payload); + sprintf(buf, "Min payload set to: %f [kg]", min_payload); + print_debug(buf); } if (ros::param::get("~max_payload", max_payload)) { - ROS_DEBUG("Max payload set to: %f [kg]", max_payload); + sprintf(buf, "Max payload set to: %f [kg]", max_payload); + print_debug(buf); } robot_.setMinPayload(min_payload); robot_.setMaxPayload(max_payload); - ROS_DEBUG("Bounds for set_payload service calls: [%f, %f]", min_payload, - max_payload); + sprintf(buf, "Bounds for set_payload service calls: [%f, %f]", + min_payload, max_payload); + print_debug(buf); if (robot_.start()) { if (use_ros_control_) { ros_control_thread_ = new std::thread( boost::bind(&RosWrapper::rosControlLoop, this)); - ROS_DEBUG( + print_debug( "The control thread for this driver has been started"); } else { //register the goal and feedback callbacks @@ -133,7 +140,7 @@ public: //subscribe to the data topic of interest rt_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishRTMsg, this)); - ROS_DEBUG("The action server for this driver has been started"); + print_debug("The action server for this driver has been started"); } mb_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishMbMsg, this)); @@ -156,7 +163,7 @@ public: } private: void goalCB() { - ROS_INFO("on_goal"); + print_info("on_goal"); actionlib::SimpleActionServer::GoalConstPtr goal = as_.acceptNewGoal(); @@ -172,21 +179,21 @@ private: "Received a goal with incorrect joint names: " + outp_joint_names; as_.setAborted(result_, result_.error_string); - ROS_ERROR("%s", result_.error_string.c_str()); + print_error(result_.error_string); } if (!has_velocities()) { result_.error_code = result_.INVALID_GOAL; result_.error_string = "Received a goal without velocities"; as_.setAborted(result_, result_.error_string); - ROS_ERROR("%s", result_.error_string.c_str()); + print_error(result_.error_string); } if (!traj_is_finite()) { result_.error_string = "Received a goal with infinites or NaNs"; result_.error_code = result_.INVALID_GOAL; as_.setAborted(result_, result_.error_string); - ROS_ERROR("%s", result_.error_string.c_str()); + print_error(result_.error_string); } if (!has_limited_velocities()) { @@ -194,7 +201,7 @@ private: result_.error_string = "Received a goal with velocities that are higher than %f", max_velocity_; as_.setAborted(result_, result_.error_string); - ROS_ERROR("%s", result_.error_string.c_str()); + print_error(result_.error_string); } reorder_traj_joints(goal_.trajectory); @@ -213,7 +220,7 @@ private: } void preemptCB() { - ROS_INFO("on_cancel"); + print_info("on_cancel"); // set the action state to preempted robot_.stopTraj(); as_.setPreempted(); @@ -481,15 +488,15 @@ int main(int argc, char **argv) { ros::init(argc, argv, "ur_driver"); ros::NodeHandle nh; if (ros::param::get("use_sim_time", use_sim_time)) { - ROS_WARN("use_sim_time is set!!"); + print_warning("use_sim_time is set!!"); } if (!(ros::param::get("~robot_ip_address", host))) { if (argc > 1) { - ROS_WARN( + print_warning( "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED"); host = argv[1]; } else { - ROS_FATAL( + print_fatal( "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); exit(1); }