diff --git a/CMakeLists.txt b/CMakeLists.txt index 644991b..846a846 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp sensor_msgs + std_msgs trajectory_msgs ur_msgs ) diff --git a/package.xml b/package.xml index 36a3ac0..73ac34b 100644 --- a/package.xml +++ b/package.xml @@ -45,6 +45,7 @@ geometry_msgs roscpp sensor_msgs + std_msgs trajectory_msgs ur_msgs actionlib @@ -52,6 +53,7 @@ geometry_msgs roscpp sensor_msgs + std_msgs trajectory_msgs ur_msgs diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index ec3010d..70ffe36 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -31,6 +31,7 @@ #include "ur_msgs/SetPayloadResponse.h" #include "ur_msgs/SetIORequest.h" #include "ur_msgs/SetIOResponse.h" +#include "std_msgs/String.h" class RosWrapper { protected: @@ -42,9 +43,10 @@ protected: actionlib::SimpleActionServer::Goal goal_; control_msgs::FollowJointTrajectoryFeedback feedback_; control_msgs::FollowJointTrajectoryResult result_; - ros::Subscriber speedSub_; - ros::ServiceServer ioSrv_; - ros::ServiceServer payloadSrv_; + ros::Subscriber speed_sub_; + ros::Subscriber urscript_sub_; + ros::ServiceServer io_srv_; + ros::ServiceServer payload_srv_; std::thread* rt_publish_thread_; double io_flag_delay_; double max_velocity_; @@ -52,8 +54,9 @@ protected: public: RosWrapper(std::string host) : - as_(nh_, "follow_joint_trajectory", false), robot_(rt_msg_cond_, msg_cond_, host), io_flag_delay_( - 0.05), joint_offsets_(6, 0.0) { + as_(nh_, "follow_joint_trajectory", false), robot_(rt_msg_cond_, + msg_cond_, host), io_flag_delay_(0.05), joint_offsets_(6, + 0.0) { std::string joint_prefix = ""; std::vector joint_names; @@ -103,12 +106,14 @@ public: as_.start(); //subscribe to the data topic of interest - speedSub_ = nh_.subscribe("joint_speed", 1, &RosWrapper::speedInterface, - this); + speed_sub_ = nh_.subscribe("joint_speed", 1, + &RosWrapper::speedInterface, this); + urscript_sub_ = nh_.subscribe("ur_driver/URSCcript", 1, + &RosWrapper::urscriptInterface, this); - ioSrv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, + io_srv_ = nh_.advertiseService("ur_driver/set_io", &RosWrapper::setIO, this); - payloadSrv_ = nh_.advertiseService("ur_driver/set_payload", + payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); rt_publish_thread_ = new std::thread( @@ -314,6 +319,11 @@ private: msg->points[0].velocities[3], msg->points[0].velocities[4], msg->points[0].velocities[5], acc); + } + void urscriptInterface(const std_msgs::String::ConstPtr& msg) { + + robot_.rt_interface_->addCommandToQueue(msg->data); + } void publishRTMsg() {