From b98e57a475c7d74ce8690b91607708e32cff1006 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 10 Sep 2015 10:37:52 +0200 Subject: [PATCH] Added IO and payload service --- include/ur_modern_driver/ur_driver.h | 7 ++- src/ur_driver.cpp | 49 +++++++++++++++--- src/ur_ros_wrapper.cpp | 74 +++++++++++++++++++--------- 3 files changed, 101 insertions(+), 29 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index cec3fdf..3ecb547 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -26,7 +26,6 @@ public: UrRealtimeCommunication* rt_interface_; UrDriver(std::condition_variable& msg_cond, std::string host, - std::vector joint_names, unsigned int safety_count_max = 12); void start(); void halt(); @@ -44,6 +43,12 @@ public: std::vector getJointNames(); void setJointNames(std::vector jn); + void setToolVoltage(unsigned int v); + void setFlag(unsigned int n, bool b); + void setDigitalOut(unsigned int n, bool b); + void setAnalogOut(unsigned int n, double f); + void setPayloaf(double m); + }; #endif /* UR_DRIVER_H_ */ diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index 6db39b1..c4acc46 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -12,11 +12,10 @@ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host, - std::vector joint_names, unsigned int safety_count_max) { + unsigned int safety_count_max) { rt_interface_ = new UrRealtimeCommunication(msg_cond, host, safety_count_max); maximum_time_step_ = 0.08; - joint_names_ = joint_names; } @@ -56,9 +55,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++) { @@ -66,7 +65,8 @@ void UrDriver::addTraj(std::vector inp_timestamps, j += 1; } positions.push_back( - UrDriver::interp_cubic(timestamps[i] - inp_timestamps[j-1], inp_timestamps[j] - inp_timestamps[j-1], + UrDriver::interp_cubic(timestamps[i] - 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])); } @@ -112,3 +112,40 @@ std::vector UrDriver::getJointNames() { void UrDriver::setJointNames(std::vector jn) { joint_names_ = jn; } + +void UrDriver::setToolVoltage(unsigned int v) { + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v); + printf("%s", buf); + rt_interface_->addCommandToQueue(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"); + printf("%s", buf); + rt_interface_->addCommandToQueue(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"); + printf("%s", buf); + rt_interface_->addCommandToQueue(buf); + +} +void UrDriver::setAnalogOut(unsigned int n, double f) { + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d, %1.4f)\nend\n", n, f); + printf("%s", buf); + rt_interface_->addCommandToQueue(buf); +} + +void UrDriver::setPayloaf(double m){ + char buf[256]; + sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m); + printf("%s", buf); + rt_interface_->addCommandToQueue(buf); + +} diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 490da76..48c9a4c 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -24,6 +24,11 @@ #include "control_msgs/FollowJointTrajectoryAction.h" #include "actionlib/server/simple_action_server.h" #include "trajectory_msgs/JointTrajectoryPoint.h" +#include "ur_msgs/SetIO.h" +#include "ur_msgs/SetPayloadRequest.h" +#include "ur_msgs/SetPayloadResponse.h" +#include "ur_msgs/SetIORequest.h" +#include "ur_msgs/SetIOResponse.h" class RosWrapper { protected: @@ -37,15 +42,29 @@ protected: std::thread* rt_publish_thread_; std::condition_variable msg_cond_; public: - RosWrapper(std::string host, std::vector joint_names) : - as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host, - joint_names) { + RosWrapper(std::string host) : + as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host) { + + std::string joint_prefix = ""; + std::vector joint_names; + + if (ros::param::get("~prefix", joint_prefix)) { + ROS_INFO("Setting prefix to %s", joint_prefix.c_str()); + } + joint_names.push_back(joint_prefix + "shoulder_pan_joint"); + joint_names.push_back(joint_prefix + "shoulder_lift_joint"); + joint_names.push_back(joint_prefix + "elbow_joint"); + joint_names.push_back(joint_prefix + "wrist_1_joint"); + joint_names.push_back(joint_prefix + "wrist_2_joint"); + joint_names.push_back(joint_prefix + "wrist_3_joint"); + robot_.setJointNames(joint_names); + + robot_.start(); //register the goal and feedback callbacks as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this)); as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this)); - robot_.start(); as_.start(); //subscribe to the data topic of interest @@ -58,6 +77,12 @@ public: } + void halt() { + robot_.halt(); + rt_publish_thread_->join(); + + } +private: void goalCB() { ROS_INFO("on_goal"); @@ -104,12 +129,29 @@ public: as_.setPreempted(); } - void halt() { - robot_.halt(); - rt_publish_thread_->join(); - + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) { + resp.success = true; + if (req.fun == ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT) { + robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false); + } else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_FLAG) { + robot_.setFlag(req.pin, req.state > 0.0 ? true : false); + } else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT) { + robot_.setAnalogOut(req.pin, req.state); + } else if (req.fun == ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE) { + robot_.setToolVoltage((int) req.state); + } else { + resp.success = false; + } + return resp.success; } -private: + + bool setPayloaf(ur_msgs::SetPayloadRequest& req, + ur_msgs::SetPayloadResponse& resp) { + robot_.setPayloaf(req.payload); + resp.success = true; + return true; + } + bool validateJointNames() { std::vector actual_joint_names = robot_.getJointNames(); if (goal_.trajectory.joint_names.size() != actual_joint_names.size()) @@ -226,20 +268,8 @@ int main(int argc, char **argv) { "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip"); exit(1); } - std::string joint_prefix = ""; - std::vector joint_names; - if (ros::param::get("~prefix", joint_prefix)) { - ROS_INFO("Setting prefix to %s", joint_prefix.c_str()); - } - joint_names.push_back(joint_prefix + "shoulder_pan_joint"); - joint_names.push_back(joint_prefix + "shoulder_lift_joint"); - joint_names.push_back(joint_prefix + "elbow_joint"); - joint_names.push_back(joint_prefix + "wrist_1_joint"); - joint_names.push_back(joint_prefix + "wrist_2_joint"); - joint_names.push_back(joint_prefix + "wrist_3_joint"); - - RosWrapper interface(host, joint_names); + RosWrapper interface(host); ros::spin(); interface.halt();