From 8bf9e356f5ec1614c82e448b35a7c0f71cd574d2 Mon Sep 17 00:00:00 2001 From: Thomas Timm Andersen Date: Thu, 10 Sep 2015 14:02:10 +0200 Subject: [PATCH] Read payload and max velocity from parameter server --- include/ur_modern_driver/ur_driver.h | 2 +- src/ur_driver.cpp | 5 ++--- src/ur_ros_wrapper.cpp | 28 +++++++++++++++++++++++++++- 3 files changed, 30 insertions(+), 5 deletions(-) diff --git a/include/ur_modern_driver/ur_driver.h b/include/ur_modern_driver/ur_driver.h index 083feaa..aefb34f 100644 --- a/include/ur_modern_driver/ur_driver.h +++ b/include/ur_modern_driver/ur_driver.h @@ -29,7 +29,7 @@ public: UrRealtimeCommunication* rt_interface_; UrDriver(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max = 12); + unsigned int safety_count_max = 12, double max_time_step = 0.08, double max_vel = 10., double min_payload= 0., double max_payload=1.); void start(); void halt(); diff --git a/src/ur_driver.cpp b/src/ur_driver.cpp index c04f3ce..fdb4aa0 100644 --- a/src/ur_driver.cpp +++ b/src/ur_driver.cpp @@ -12,9 +12,8 @@ #include "ur_modern_driver/ur_driver.h" UrDriver::UrDriver(std::condition_variable& msg_cond, std::string host, - unsigned int safety_count_max) : - maximum_time_step_(0.08), maximum_velocity_(10.0), minimum_payload_( - 0.0), maximum_payload_(1.0) { + unsigned int safety_count_max, double max_time_step, double max_vel, double min_payload, double max_payload ) : + maximum_time_step_(max_time_step), maximum_velocity_(max_vel), minimum_payload_(min_payload), maximum_payload_(max_payload) { rt_interface_ = new UrRealtimeCommunication(msg_cond, host, safety_count_max); diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 08d3b5d..fff16cc 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -48,7 +48,8 @@ protected: public: RosWrapper(std::string host) : - as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host), io_flag_delay_(0.05) { + as_(nh_, "follow_joint_trajectory", false), robot_(msg_cond_, host), io_flag_delay_( + 0.05) { std::string joint_prefix = ""; std::vector joint_names; @@ -64,6 +65,31 @@ public: joint_names.push_back(joint_prefix + "wrist_3_joint"); robot_.setJointNames(joint_names); + { //Using a very high value in order to not limit execution of trajectories being sent from MoveIt! + double max_velocity = 10.; + if (ros::param::get("~max_velocity", max_velocity)) { + ROS_INFO( + "Max velocity accepted by ur_driver: %f [rad/s]", max_velocity); + robot_.setMaxVel(max_velocity); + } + } + + { //Bounds for SetPayload service + //Using a very conservative value as it should be set through the parameter server + double min_payload = 0.; + double max_payload = 1.; + if (ros::param::get("~min_payload", min_payload)) { + ROS_INFO( + "Min payload accepted by ur_driver: %f [kg]", min_payload); + } + if (ros::param::get("~max_payload", max_payload)) { + ROS_INFO( + "Max payload accepted by ur_driver: %f [kg]", max_payload); + } + robot_.setMinPayload(min_payload); + robot_.setMaxPayload(max_payload); + ROS_INFO("Bounds for set_payload service calls: [%f, %f]", min_payload, max_payload); + } robot_.start(); //register the goal and feedback callbacks