From 7fdff1c66dc9bc1d4975551733745b7a6b8e1279 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 8 Oct 2015 15:06:04 +0200 Subject: [PATCH 01/12] Publish robot_state --- src/ur_ros_wrapper.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index c782bde..28b8add 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -38,6 +38,7 @@ #include "ur_msgs/Digital.h" #include "ur_msgs/Analog.h" #include "std_msgs/String.h" +#include "std_msgs/Int32.h" #include class RosWrapper { @@ -54,8 +55,8 @@ protected: ros::Subscriber urscript_sub_; ros::ServiceServer io_srv_; ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; @@ -423,6 +424,8 @@ private: "joint_states", 1); ros::Publisher wrench_pub = nh_.advertise( "wrench", 1); + ros::Publisher robot_state_pub = nh_.advertise( + "ur_driver/robot_state", 1); while (ros::ok()) { sensor_msgs::JointState joint_msg; joint_msg.name = robot_.getJointNames(); @@ -453,6 +456,11 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); + //Publish robot state + std_msgs::Int32 robot_state_msg; + robot_state_msg.data = (int) robot_.rt_interface_->robot_state_->getRobotMode(); + robot_state_pub.publish(robot_state_msg); + robot_.rt_interface_->robot_state_->setDataPublished(); } From a18aae9154df5f2be0c92c0fa81da19c05328a49 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 8 Oct 2015 17:03:12 +0200 Subject: [PATCH 02/12] Publisher driver state as diagnostics --- src/ur_ros_wrapper.cpp | 72 ++++++++++++++++++++++++++++++++++++++---- 1 file changed, 65 insertions(+), 7 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 28b8add..8796d2c 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -38,8 +38,38 @@ #include "ur_msgs/Digital.h" #include "ur_msgs/Analog.h" #include "std_msgs/String.h" -#include "std_msgs/Int32.h" #include +#include +#include + +enum RobotMode { + ROBOT_MODE_DISCONNECTED, + ROBOT_MODE_CONFIRM_SAFETY, + ROBOT_MODE_BOOTING, + ROBOT_MODE_POWER_OFF, + ROBOT_MODE_POWER_ON, + ROBOT_MODE_IDLE, + ROBOT_MODE_BACKDRIVE, + ROBOT_MODE_RUNNING, + ROBOT_MODE_UPDATING_FIRMWARE +}; + +enum SafetyMode { + SAFETY_MODE_NORMAL, + SAFETY_MODE_REDUCED, + SAFETY_MODE_PROTECTIVE_STOP, + SAFETY_MODE_RECOVERY, + SAFETY_MODE_SAFEGUARD_STOP, + SAFETY_MODE_SYSTEM_EMERGENCY_STOP, + SAFETY_MODE_ROBOT_EMERGENCY_STOP, + SAFETY_MODE_VIOLATION, + SAFETY_MODE_FAULT +}; + +struct DriverStatus{ + RobotMode robot_mode; + SafetyMode safety_mode; +}; class RosWrapper { protected: @@ -64,6 +94,8 @@ protected: std::thread* ros_control_thread_; boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; + diagnostic_updater::Updater updater_; + DriverStatus driver_status_; public: RosWrapper(std::string host) : @@ -75,6 +107,9 @@ public: std::vector joint_names; char buf[256]; + updater_.setHardwareID("ur_driver"); + updater_.add("Status updater", this, &RosWrapper::driverDiagnostic); + if (ros::param::get("~prefix", joint_prefix)) { sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); @@ -171,6 +206,26 @@ public: } private: + void driverDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){ + + if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && + (driver_status_.safety_mode == SAFETY_MODE_NORMAL || driver_status_.safety_mode == SAFETY_MODE_REDUCED)){ + //Robot is running and in normal safety mode + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver OK"); + } + else if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && + (driver_status_.safety_mode != SAFETY_MODE_NORMAL || driver_status_.safety_mode != SAFETY_MODE_REDUCED)){ + //Robot is running and in some safety stop + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Driver WARNING"); + } + else { + //Robot is not running + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver ERROR"); + } + stat.add("Robot mode", driver_status_.robot_mode); + stat.add("Safety mode", driver_status_.safety_mode); + } + void goalCB() { print_info("on_goal"); @@ -424,8 +479,6 @@ private: "joint_states", 1); ros::Publisher wrench_pub = nh_.advertise( "wrench", 1); - ros::Publisher robot_state_pub = nh_.advertise( - "ur_driver/robot_state", 1); while (ros::ok()) { sensor_msgs::JointState joint_msg; joint_msg.name = robot_.getJointNames(); @@ -456,10 +509,15 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); - //Publish robot state - std_msgs::Int32 robot_state_msg; - robot_state_msg.data = (int) robot_.rt_interface_->robot_state_->getRobotMode(); - robot_state_pub.publish(robot_state_msg); +// //Publish robot state +// std_msgs::Int32 robot_state_msg; +// robot_state_msg.data = (int) robot_.rt_interface_->robot_state_->getRobotMode(); +// robot_state_pub.publish(robot_state_msg); + + //Update diagnostics + driver_status_.robot_mode = static_cast( robot_.rt_interface_->robot_state_->getRobotMode() ); + driver_status_.safety_mode = static_cast( robot_.rt_interface_->robot_state_->getSafety_mode() ); + updater_.update(); robot_.rt_interface_->robot_state_->setDataPublished(); From 5c7daf43650f454e08033e2241324bf00f7c9505 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 8 Oct 2015 17:03:36 +0200 Subject: [PATCH 03/12] Remove obsolete robot state publisher --- src/ur_ros_wrapper.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 8796d2c..6eea140 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -509,11 +509,6 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); -// //Publish robot state -// std_msgs::Int32 robot_state_msg; -// robot_state_msg.data = (int) robot_.rt_interface_->robot_state_->getRobotMode(); -// robot_state_pub.publish(robot_state_msg); - //Update diagnostics driver_status_.robot_mode = static_cast( robot_.rt_interface_->robot_state_->getRobotMode() ); driver_status_.safety_mode = static_cast( robot_.rt_interface_->robot_state_->getSafety_mode() ); From 644ea2491c406ce68b651cd1d11366bfd3bf96cf Mon Sep 17 00:00:00 2001 From: Simon Date: Fri, 9 Oct 2015 16:44:00 +0200 Subject: [PATCH 04/12] Rollback to original (diagnostics is now in separate branch) --- src/ur_ros_wrapper.cpp | 65 ++---------------------------------------- 1 file changed, 2 insertions(+), 63 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 6eea140..c782bde 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -39,37 +39,6 @@ #include "ur_msgs/Analog.h" #include "std_msgs/String.h" #include -#include -#include - -enum RobotMode { - ROBOT_MODE_DISCONNECTED, - ROBOT_MODE_CONFIRM_SAFETY, - ROBOT_MODE_BOOTING, - ROBOT_MODE_POWER_OFF, - ROBOT_MODE_POWER_ON, - ROBOT_MODE_IDLE, - ROBOT_MODE_BACKDRIVE, - ROBOT_MODE_RUNNING, - ROBOT_MODE_UPDATING_FIRMWARE -}; - -enum SafetyMode { - SAFETY_MODE_NORMAL, - SAFETY_MODE_REDUCED, - SAFETY_MODE_PROTECTIVE_STOP, - SAFETY_MODE_RECOVERY, - SAFETY_MODE_SAFEGUARD_STOP, - SAFETY_MODE_SYSTEM_EMERGENCY_STOP, - SAFETY_MODE_ROBOT_EMERGENCY_STOP, - SAFETY_MODE_VIOLATION, - SAFETY_MODE_FAULT -}; - -struct DriverStatus{ - RobotMode robot_mode; - SafetyMode safety_mode; -}; class RosWrapper { protected: @@ -85,8 +54,8 @@ protected: ros::Subscriber urscript_sub_; ros::ServiceServer io_srv_; ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; @@ -94,8 +63,6 @@ protected: std::thread* ros_control_thread_; boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; - diagnostic_updater::Updater updater_; - DriverStatus driver_status_; public: RosWrapper(std::string host) : @@ -107,9 +74,6 @@ public: std::vector joint_names; char buf[256]; - updater_.setHardwareID("ur_driver"); - updater_.add("Status updater", this, &RosWrapper::driverDiagnostic); - if (ros::param::get("~prefix", joint_prefix)) { sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); @@ -206,26 +170,6 @@ public: } private: - void driverDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){ - - if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && - (driver_status_.safety_mode == SAFETY_MODE_NORMAL || driver_status_.safety_mode == SAFETY_MODE_REDUCED)){ - //Robot is running and in normal safety mode - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver OK"); - } - else if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && - (driver_status_.safety_mode != SAFETY_MODE_NORMAL || driver_status_.safety_mode != SAFETY_MODE_REDUCED)){ - //Robot is running and in some safety stop - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Driver WARNING"); - } - else { - //Robot is not running - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver ERROR"); - } - stat.add("Robot mode", driver_status_.robot_mode); - stat.add("Safety mode", driver_status_.safety_mode); - } - void goalCB() { print_info("on_goal"); @@ -509,11 +453,6 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); - //Update diagnostics - driver_status_.robot_mode = static_cast( robot_.rt_interface_->robot_state_->getRobotMode() ); - driver_status_.safety_mode = static_cast( robot_.rt_interface_->robot_state_->getSafety_mode() ); - updater_.update(); - robot_.rt_interface_->robot_state_->setDataPublished(); } From c18ff438ca4d35985cb4621f31cfbdd683cac479 Mon Sep 17 00:00:00 2001 From: Simon Jansen Date: Fri, 16 Oct 2015 13:27:42 +0200 Subject: [PATCH 05/12] Add install targets --- CMakeLists.txt | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1a9702e..03abbfa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,6 +174,15 @@ target_link_libraries(ur_driver ## Install ## ############# +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +## Mark executables and/or libraries for installation +install(TARGETS ur_driver ur_hardware_interface +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html From 777cf2fe71ad5b04e98dd1cea3f2377e71f5a8c2 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 12 Nov 2015 13:45:07 +0100 Subject: [PATCH 06/12] Revert "Rollback to original (diagnostics is now in separate branch)" This reverts commit 644ea2491c406ce68b651cd1d11366bfd3bf96cf. --- src/ur_ros_wrapper.cpp | 65 ++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 63 insertions(+), 2 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index c782bde..6eea140 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -39,6 +39,37 @@ #include "ur_msgs/Analog.h" #include "std_msgs/String.h" #include +#include +#include + +enum RobotMode { + ROBOT_MODE_DISCONNECTED, + ROBOT_MODE_CONFIRM_SAFETY, + ROBOT_MODE_BOOTING, + ROBOT_MODE_POWER_OFF, + ROBOT_MODE_POWER_ON, + ROBOT_MODE_IDLE, + ROBOT_MODE_BACKDRIVE, + ROBOT_MODE_RUNNING, + ROBOT_MODE_UPDATING_FIRMWARE +}; + +enum SafetyMode { + SAFETY_MODE_NORMAL, + SAFETY_MODE_REDUCED, + SAFETY_MODE_PROTECTIVE_STOP, + SAFETY_MODE_RECOVERY, + SAFETY_MODE_SAFEGUARD_STOP, + SAFETY_MODE_SYSTEM_EMERGENCY_STOP, + SAFETY_MODE_ROBOT_EMERGENCY_STOP, + SAFETY_MODE_VIOLATION, + SAFETY_MODE_FAULT +}; + +struct DriverStatus{ + RobotMode robot_mode; + SafetyMode safety_mode; +}; class RosWrapper { protected: @@ -54,8 +85,8 @@ protected: ros::Subscriber urscript_sub_; ros::ServiceServer io_srv_; ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; @@ -63,6 +94,8 @@ protected: std::thread* ros_control_thread_; boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; + diagnostic_updater::Updater updater_; + DriverStatus driver_status_; public: RosWrapper(std::string host) : @@ -74,6 +107,9 @@ public: std::vector joint_names; char buf[256]; + updater_.setHardwareID("ur_driver"); + updater_.add("Status updater", this, &RosWrapper::driverDiagnostic); + if (ros::param::get("~prefix", joint_prefix)) { sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); @@ -170,6 +206,26 @@ public: } private: + void driverDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){ + + if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && + (driver_status_.safety_mode == SAFETY_MODE_NORMAL || driver_status_.safety_mode == SAFETY_MODE_REDUCED)){ + //Robot is running and in normal safety mode + stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver OK"); + } + else if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && + (driver_status_.safety_mode != SAFETY_MODE_NORMAL || driver_status_.safety_mode != SAFETY_MODE_REDUCED)){ + //Robot is running and in some safety stop + stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Driver WARNING"); + } + else { + //Robot is not running + stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver ERROR"); + } + stat.add("Robot mode", driver_status_.robot_mode); + stat.add("Safety mode", driver_status_.safety_mode); + } + void goalCB() { print_info("on_goal"); @@ -453,6 +509,11 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); + //Update diagnostics + driver_status_.robot_mode = static_cast( robot_.rt_interface_->robot_state_->getRobotMode() ); + driver_status_.safety_mode = static_cast( robot_.rt_interface_->robot_state_->getSafety_mode() ); + updater_.update(); + robot_.rt_interface_->robot_state_->setDataPublished(); } From 9d19fa3abee9220f0081831639e32a39568ec8c6 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 12 Nov 2015 13:46:37 +0100 Subject: [PATCH 07/12] Revert "Add install targets" This reverts commit c18ff438ca4d35985cb4621f31cfbdd683cac479. --- CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 03abbfa..1a9702e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -174,15 +174,6 @@ target_link_libraries(ur_driver ## Install ## ############# -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -## Mark executables and/or libraries for installation -install(TARGETS ur_driver ur_hardware_interface -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html From 3515b2cf4005121e1f2c93818d53419b522f0404 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 12 Nov 2015 13:59:01 +0100 Subject: [PATCH 08/12] Revert "Add install targets" -> rollback to original This reverts commit c18ff438ca4d35985cb4621f31cfbdd683cac479. --- src/ur_ros_wrapper.cpp | 65 ++---------------------------------------- 1 file changed, 2 insertions(+), 63 deletions(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 6eea140..c782bde 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -39,37 +39,6 @@ #include "ur_msgs/Analog.h" #include "std_msgs/String.h" #include -#include -#include - -enum RobotMode { - ROBOT_MODE_DISCONNECTED, - ROBOT_MODE_CONFIRM_SAFETY, - ROBOT_MODE_BOOTING, - ROBOT_MODE_POWER_OFF, - ROBOT_MODE_POWER_ON, - ROBOT_MODE_IDLE, - ROBOT_MODE_BACKDRIVE, - ROBOT_MODE_RUNNING, - ROBOT_MODE_UPDATING_FIRMWARE -}; - -enum SafetyMode { - SAFETY_MODE_NORMAL, - SAFETY_MODE_REDUCED, - SAFETY_MODE_PROTECTIVE_STOP, - SAFETY_MODE_RECOVERY, - SAFETY_MODE_SAFEGUARD_STOP, - SAFETY_MODE_SYSTEM_EMERGENCY_STOP, - SAFETY_MODE_ROBOT_EMERGENCY_STOP, - SAFETY_MODE_VIOLATION, - SAFETY_MODE_FAULT -}; - -struct DriverStatus{ - RobotMode robot_mode; - SafetyMode safety_mode; -}; class RosWrapper { protected: @@ -85,8 +54,8 @@ protected: ros::Subscriber urscript_sub_; ros::ServiceServer io_srv_; ros::ServiceServer payload_srv_; - std::thread* rt_publish_thread_; - std::thread* mb_publish_thread_; + std::thread* rt_publish_thread_; + std::thread* mb_publish_thread_; double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; @@ -94,8 +63,6 @@ protected: std::thread* ros_control_thread_; boost::shared_ptr hardware_interface_; boost::shared_ptr controller_manager_; - diagnostic_updater::Updater updater_; - DriverStatus driver_status_; public: RosWrapper(std::string host) : @@ -107,9 +74,6 @@ public: std::vector joint_names; char buf[256]; - updater_.setHardwareID("ur_driver"); - updater_.add("Status updater", this, &RosWrapper::driverDiagnostic); - if (ros::param::get("~prefix", joint_prefix)) { sprintf(buf, "Setting prefix to %s", joint_prefix.c_str()); print_info(buf); @@ -206,26 +170,6 @@ public: } private: - void driverDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat){ - - if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && - (driver_status_.safety_mode == SAFETY_MODE_NORMAL || driver_status_.safety_mode == SAFETY_MODE_REDUCED)){ - //Robot is running and in normal safety mode - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Driver OK"); - } - else if (driver_status_.robot_mode == ROBOT_MODE_RUNNING && - (driver_status_.safety_mode != SAFETY_MODE_NORMAL || driver_status_.safety_mode != SAFETY_MODE_REDUCED)){ - //Robot is running and in some safety stop - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Driver WARNING"); - } - else { - //Robot is not running - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Driver ERROR"); - } - stat.add("Robot mode", driver_status_.robot_mode); - stat.add("Safety mode", driver_status_.safety_mode); - } - void goalCB() { print_info("on_goal"); @@ -509,11 +453,6 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); - //Update diagnostics - driver_status_.robot_mode = static_cast( robot_.rt_interface_->robot_state_->getRobotMode() ); - driver_status_.safety_mode = static_cast( robot_.rt_interface_->robot_state_->getSafety_mode() ); - updater_.update(); - robot_.rt_interface_->robot_state_->setDataPublished(); } From 4cdceac79187dee077163127ca76d218b94b1a76 Mon Sep 17 00:00:00 2001 From: Simon Date: Thu, 12 Nov 2015 17:22:20 +0100 Subject: [PATCH 09/12] Publish actual tool pose as pose msg and tf --- CMakeLists.txt | 1 + package.xml | 2 ++ src/ur_ros_wrapper.cpp | 38 ++++++++++++++++++++++++++++++++++++++ 3 files changed, 41 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index cfd2a37..e9f3b83 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS std_msgs trajectory_msgs ur_msgs + tf ) ## System dependencies are found with CMake's conventions diff --git a/package.xml b/package.xml index 24faa14..1d45aaa 100644 --- a/package.xml +++ b/package.xml @@ -50,6 +50,7 @@ std_msgs trajectory_msgs ur_msgs + tf hardware_interface controller_manager ros_controllers @@ -62,6 +63,7 @@ trajectory_msgs ur_msgs ur_description + tf diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index b9f15ca..017c874 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -33,6 +33,7 @@ #include #include "sensor_msgs/JointState.h" #include "geometry_msgs/WrenchStamped.h" +#include "geometry_msgs/PoseStamped.h" #include "control_msgs/FollowJointTrajectoryAction.h" #include "actionlib/server/action_server.h" #include "actionlib/server/server_goal_handle.h" @@ -49,6 +50,10 @@ #include "std_msgs/String.h" #include +/// TF +#include +#include + class RosWrapper { protected: UrDriver robot_; @@ -541,10 +546,13 @@ private: "joint_states", 1); ros::Publisher wrench_pub = nh_.advertise( "wrench", 1); + ros::Publisher tool_pose_pub = nh_.advertise("tool_pose", 1); + static tf::TransformBroadcaster br; while (ros::ok()) { sensor_msgs::JointState joint_msg; joint_msg.name = robot_.getJointNames(); geometry_msgs::WrenchStamped wrench_msg; + geometry_msgs::PoseStamped tool_pose_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_->getDataPublished()) { @@ -571,6 +579,36 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); + // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation + std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); + tool_pose_msg.pose.position.x = tool_vector_actual[0]; + tool_pose_msg.pose.position.y = tool_vector_actual[1]; + tool_pose_msg.pose.position.z = tool_vector_actual[2]; + + tf::Quaternion quat; +// quat.setEuler(tool_vector_actual[4], tool_vector_actual[3], tool_vector_actual[5]); + double rx = tool_vector_actual[3]; + double ry = tool_vector_actual[4]; + double rz = tool_vector_actual[5]; + double angle = std::sqrt(rx*rx + ry*ry + rz*rz); + rx = rx/angle; + ry = ry/angle; + rz = rz/angle; + quat.setRotation(tf::Vector3(rx, ry, rz), angle); +// quat.setRotation(tf::Vector3(1,0,0), tool_vector_actual[3]); +// quat.setRotation(tf::Vector3(0,1,0), tool_vector_actual[4]); +// quat.setRotation(tf::Vector3(0,0,1), tool_vector_actual[5]); +// quat.setRPY(tool_vector_actual[3], tool_vector_actual[4], tool_vector_actual[5]); + tf::quaternionTFToMsg(quat, tool_pose_msg.pose.orientation); + + tool_pose_pub.publish(tool_pose_msg); + + tf::Transform transform; + transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2])); + transform.setRotation(quat); + + br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "philips/ur_base_link", "philips/ur_ee_link_calibrated")); + robot_.rt_interface_->robot_state_->setDataPublished(); } From d24019b0d1627940fbbd67fe8e081296e0888d9a Mon Sep 17 00:00:00 2001 From: Simon Date: Fri, 13 Nov 2015 15:00:52 +0100 Subject: [PATCH 10/12] Publish tool twist and get base and tool frame names from parameters --- launch/ur_common.launch | 5 +++- src/ur_ros_wrapper.cpp | 52 ++++++++++++++++++++++++++--------------- 2 files changed, 37 insertions(+), 20 deletions(-) diff --git a/launch/ur_common.launch b/launch/ur_common.launch index 6cd06dd..565fa86 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -12,7 +12,8 @@ - + + @@ -27,5 +28,7 @@ + + diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 017c874..09a9526 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -74,6 +74,8 @@ protected: double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; + std::string base_frame_; + std::string tool_frame_; bool use_ros_control_; std::thread* ros_control_thread_; boost::shared_ptr hardware_interface_; @@ -154,6 +156,18 @@ public: } robot_.setServojTime(servoj_time); + //Base and tool frames + base_frame_ = "base"; + tool_frame_ = "tool0_controller"; + if (ros::param::get("~base_frame", base_frame_)) { + sprintf(buf, "Base frame set to: %s", base_frame_.c_str()); + print_debug(buf); + } + if (ros::param::get("~tool_frame", tool_frame_)) { + sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str()); + print_debug(buf); + } + if (robot_.start()) { if (use_ros_control_) { ros_control_thread_ = new std::thread( @@ -546,7 +560,7 @@ private: "joint_states", 1); ros::Publisher wrench_pub = nh_.advertise( "wrench", 1); - ros::Publisher tool_pose_pub = nh_.advertise("tool_pose", 1); + ros::Publisher tool_vel_pub = nh_.advertise("tool_velocity", 1); static tf::TransformBroadcaster br; while (ros::ok()) { sensor_msgs::JointState joint_msg; @@ -581,36 +595,36 @@ private: // Tool vector: Actual Cartesian coordinates of the tool: (x,y,z,rx,ry,rz), where rx, ry and rz is a rotation vector representation of the tool orientation std::vector tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual(); - tool_pose_msg.pose.position.x = tool_vector_actual[0]; - tool_pose_msg.pose.position.y = tool_vector_actual[1]; - tool_pose_msg.pose.position.z = tool_vector_actual[2]; + //Create quaternion tf::Quaternion quat; -// quat.setEuler(tool_vector_actual[4], tool_vector_actual[3], tool_vector_actual[5]); double rx = tool_vector_actual[3]; double ry = tool_vector_actual[4]; double rz = tool_vector_actual[5]; - double angle = std::sqrt(rx*rx + ry*ry + rz*rz); - rx = rx/angle; - ry = ry/angle; - rz = rz/angle; - quat.setRotation(tf::Vector3(rx, ry, rz), angle); -// quat.setRotation(tf::Vector3(1,0,0), tool_vector_actual[3]); -// quat.setRotation(tf::Vector3(0,1,0), tool_vector_actual[4]); -// quat.setRotation(tf::Vector3(0,0,1), tool_vector_actual[5]); -// quat.setRPY(tool_vector_actual[3], tool_vector_actual[4], tool_vector_actual[5]); - tf::quaternionTFToMsg(quat, tool_pose_msg.pose.orientation); - - tool_pose_pub.publish(tool_pose_msg); + double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); + quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle); + //Create and broadcast transform tf::Transform transform; transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2])); transform.setRotation(quat); + br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_)); - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "philips/ur_base_link", "philips/ur_ee_link_calibrated")); + //Publish tool velocity + std::vector tcp_speed = + robot_.rt_interface_->robot_state_->getTcpSpeedActual(); + geometry_msgs::TwistStamped tool_twist; + tool_twist.header.frame_id = tool_frame_; + tool_twist.header.stamp = joint_msg.header.stamp; + tool_twist.twist.linear.x = tcp_speed[0]; + tool_twist.twist.linear.y = tcp_speed[1]; + tool_twist.twist.linear.z = tcp_speed[2]; + tool_twist.twist.angular.x = tcp_speed[3]; + tool_twist.twist.angular.y = tcp_speed[4]; + tool_twist.twist.angular.z = tcp_speed[5]; + tool_vel_pub.publish(tool_twist); robot_.rt_interface_->robot_state_->setDataPublished(); - } } From 737c8310d52af6f2ddc1ec5c7303f3505f7d56c4 Mon Sep 17 00:00:00 2001 From: Simon Date: Mon, 16 Nov 2015 12:52:15 +0100 Subject: [PATCH 11/12] Catch NaN error if angles are zero --- src/ur_ros_wrapper.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 09a9526..24d805e 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -602,7 +602,11 @@ private: double ry = tool_vector_actual[4]; double rz = tool_vector_actual[5]; double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2)); - quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle); + if (angle < 1e-16) { + quat.setValue(0, 0, 0, 1); + } else { + quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle); + } //Create and broadcast transform tf::Transform transform; From dc7204fe6e546c5924fa6d82aa9b1222f24fe88e Mon Sep 17 00:00:00 2001 From: Simon Date: Mon, 16 Nov 2015 12:52:42 +0100 Subject: [PATCH 12/12] Set reference frame for twist to base_frame_ --- src/ur_ros_wrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index 24d805e..c95aa6b 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -618,7 +618,7 @@ private: std::vector tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual(); geometry_msgs::TwistStamped tool_twist; - tool_twist.header.frame_id = tool_frame_; + tool_twist.header.frame_id = base_frame_; tool_twist.header.stamp = joint_msg.header.stamp; tool_twist.twist.linear.x = tcp_speed[0]; tool_twist.twist.linear.y = tcp_speed[1];