diff --git a/CMakeLists.txt b/CMakeLists.txt index f77beb0..078d776 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,8 @@ add_definitions( -DROS_BUILD ) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + hardware_interface + controller_manager actionlib control_msgs geometry_msgs @@ -107,8 +109,8 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS include # LIBRARIES ur_modern_driver -# CATKIN_DEPENDS actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs -# DEPENDS system_lib + CATKIN_DEPENDS hardware_interface controller_manager actionlib control_msgs geometry_msgs roscpp sensor_msgs trajectory_msgs ur_msgs + DEPENDS ur_hardware_interface ) ########### @@ -135,9 +137,12 @@ include_directories(include ) ## Declare a C++ library -# add_library(ur_modern_driver -# src/${PROJECT_NAME}/ur_modern_driver.cpp -# ) + +# Hardware Interface +add_library(ur_hardware_interface src/ur_hardware_interface.cpp) +target_link_libraries(ur_hardware_interface + ${catkin_LIBRARIES} +) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries @@ -160,7 +165,8 @@ add_executable(ur_driver ${${PROJECT_NAME}_SOURCES}) ## Specify libraries to link a library or executable target against target_link_libraries(ur_driver - ${catkin_LIBRARIES} + ur_hardware_interface + ${catkin_LIBRARIES} ) ############# diff --git a/config/ur10_controllers.yaml b/config/ur10_controllers.yaml new file mode 100644 index 0000000..1389f4d --- /dev/null +++ b/config/ur10_controllers.yaml @@ -0,0 +1,55 @@ +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_trajectory_controller: + type: velocity_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: + # joint1: {p: 2.0, i: 0.0, d: 0.01, i_clamp: 1} + # joint2: {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 + diff --git a/config/ur5_controllers.yaml b/config/ur5_controllers.yaml new file mode 100644 index 0000000..0f629ea --- /dev/null +++ b/config/ur5_controllers.yaml @@ -0,0 +1,60 @@ +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +position_trajectory_controller: + type: velocity_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 + diff --git a/include/ur_modern_driver/robot_state_RT.h b/include/ur_modern_driver/robot_state_RT.h index 77823f1..ae90b71 100644 --- a/include/ur_modern_driver/robot_state_RT.h +++ b/include/ur_modern_driver/robot_state_RT.h @@ -56,7 +56,8 @@ private: std::mutex val_lock_; // Locks the variables while unpack parses data; std::condition_variable* pMsg_cond_; //Signals that new vars are available - bool new_data_available_; //to avoid spurious wakes + bool data_published_; //to avoid spurious wakes + bool controller_updated_; //to avoid spurious wakes std::vector unpackVector(uint8_t * buf, int start_index, int nr_of_vals); @@ -98,7 +99,10 @@ public: void setVersion(double ver); - void finishedReading(); + void setDataPublished(); + bool getDataPublished(); + bool getControllerUpdated(); + void setControllerUpdated(); std::vector getVActual(); void unpack(uint8_t * buf); }; diff --git a/include/ur_modern_driver/ur_hardware_interface.h b/include/ur_modern_driver/ur_hardware_interface.h new file mode 100644 index 0000000..707ad53 --- /dev/null +++ b/include/ur_modern_driver/ur_hardware_interface.h @@ -0,0 +1,117 @@ +/* + * ur_hardware_control_loop.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 + * ---------------------------------------------------------------------------- + */ + +/* 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. */ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: Example ros_control hardware interface that performs a perfect control loop for simulation + */ + +#ifndef UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H +#define UR_ROS_CONTROL_UR_HARDWARE_INTERFACE_H + +#include +#include +#include +#include +#include +#include +#include +#include "ur_driver.h" + +namespace ros_control_ur { + +// For simulation only - determines how fast a trajectory is followed +static const double POSITION_STEP_FACTOR = 1; +static const double VELOCITY_STEP_FACTOR = 1; + +/// \brief Hardware interface for a robot +class UrHardwareInterface: public hardware_interface::RobotHW { +public: + + /** + * \brief Constructor + * \param nh - Node handle for topics. + */ + UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot); + + /// \brief Initialize the hardware interface + virtual void init(); + + /// \brief Read the state from the robot hardware. + virtual void read(); + + /// \brief write the command to the robot hardware. + virtual void write(); + +protected: + + // Startup and shutdown of the internal node inside a roscpp program + ros::NodeHandle nh_; + + // Interfaces + hardware_interface::JointStateInterface joint_state_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + hardware_interface::VelocityJointInterface velocity_joint_interface_; + + // Shared memory + std::vector joint_names_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_velocity_command_; + std::size_t num_joints_; + double robot_force_[3] = {0.,0.,0.}; + double robot_torque_[3] = {0.,0.,0.}; + + // Robot API + UrDriver* robot_; + +}; +// class + +}// namespace + +#endif diff --git a/launch/ur10_ros_control.launch b/launch/ur10_ros_control.launch new file mode 100644 index 0000000..733ccfe --- /dev/null +++ b/launch/ur10_ros_control.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index fca474b..212b8e6 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -12,7 +12,7 @@ - + diff --git a/launch/ur5_ros_control.launch b/launch/ur5_ros_control.launch new file mode 100644 index 0000000..ca35ba3 --- /dev/null +++ b/launch/ur5_ros_control.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 73ac34b..24faa14 100644 --- a/package.xml +++ b/package.xml @@ -40,6 +40,8 @@ catkin + hardware_interface + controller_manager actionlib control_msgs geometry_msgs @@ -48,6 +50,9 @@ std_msgs trajectory_msgs ur_msgs + hardware_interface + controller_manager + ros_controllers actionlib control_msgs geometry_msgs @@ -56,6 +61,7 @@ std_msgs trajectory_msgs ur_msgs + ur_description diff --git a/src/robot_state_RT.cpp b/src/robot_state_RT.cpp index 602f310..273db0e 100644 --- a/src/robot_state_RT.cpp +++ b/src/robot_state_RT.cpp @@ -41,23 +41,33 @@ RobotStateRT::RobotStateRT(std::condition_variable& msg_cond) { v_robot_ = 0.0; i_robot_ = 0.0; v_actual_.assign(6, 0.0); - new_data_available_ = false; + data_published_ = false; + controller_updated_ = false; pMsg_cond_ = &msg_cond; } RobotStateRT::~RobotStateRT() { /* Make sure nobody is waiting after this thread is destroyed */ - new_data_available_ = true; + data_published_ = true; + controller_updated_ = true; pMsg_cond_->notify_all(); } -bool RobotStateRT::getNewDataAvailable() { - return new_data_available_; + +void RobotStateRT::setDataPublished() { + data_published_ = false; +} +bool RobotStateRT::getDataPublished() { + return data_published_; } -void RobotStateRT::finishedReading() { - new_data_available_ = false; +void RobotStateRT::setControllerUpdated() { + controller_updated_ = false; } +bool RobotStateRT::getControllerUpdated() { + return controller_updated_; +} + double RobotStateRT::ntohd(uint64_t nf) { double x; @@ -394,7 +404,8 @@ void RobotStateRT::unpack(uint8_t * buf) { v_actual_ = unpackVector(buf, offset, 6); } val_lock_.unlock(); - new_data_available_ = true; + controller_updated_ = true; + data_published_ = true; pMsg_cond_->notify_all(); } diff --git a/src/ur_hardware_interface.cpp b/src/ur_hardware_interface.cpp new file mode 100644 index 0000000..9705af4 --- /dev/null +++ b/src/ur_hardware_interface.cpp @@ -0,0 +1,133 @@ +/* + * ur_hardware_control_loop.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 + * ---------------------------------------------------------------------------- + */ + +/* 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. */ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2015, University of Colorado, Boulder + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Univ of CO, Boulder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman + Desc: Example ros_control hardware interface that performs a perfect control loop for simulation + */ + +#include + +namespace ros_control_ur { + +UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) : + nh_(nh), robot_(robot) { + // Initialize shared memory and interfaces here + init(); // this implementation loads from rosparam + + ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface."); +} + +void UrHardwareInterface::init() { + ROS_INFO_STREAM_NAMED("ur_hardware_interface", + "Reading rosparams from namespace: " << nh_.getNamespace()); + + // Get joint names + nh_.getParam("hardware_interface/joints", joint_names_); + if (joint_names_.size() == 0) { + ROS_FATAL_STREAM_NAMED("ur_hardware_interface", + "No joints found on parameter server for controller, did you load the proper yaml file?" << " Namespace: " << nh_.getNamespace()); + exit(-1); + } + num_joints_ = joint_names_.size(); + + // Resize vectors + joint_position_.resize(num_joints_); + joint_velocity_.resize(num_joints_); + joint_effort_.resize(num_joints_); + joint_velocity_command_.resize(num_joints_); + + // Initialize controller + for (std::size_t i = 0; i < num_joints_; ++i) { + ROS_DEBUG_STREAM_NAMED("ur_hardware_interface", + "Loading joint name: " << joint_names_[i]); + + // Create joint state interface + joint_state_interface_.registerHandle( + hardware_interface::JointStateHandle(joint_names_[i], + &joint_position_[i], &joint_velocity_[i], + &joint_effort_[i])); + + // Create velocity joint interface + velocity_joint_interface_.registerHandle( + hardware_interface::JointHandle( + joint_state_interface_.getHandle(joint_names_[i]), + &joint_velocity_command_[i])); + } + + // Create joint state interface + force_torque_interface_.registerHandle( + hardware_interface::ForceTorqueSensorHandle("wrench", "", + robot_force_, robot_torque_)); + + registerInterface(&joint_state_interface_); // From RobotHW base class. + registerInterface(&velocity_joint_interface_); // From RobotHW base class. + registerInterface(&force_torque_interface_); // From RobotHW base class. +} + +void UrHardwareInterface::read() { + std::vector pos, vel, current, tcp; + pos = robot_->rt_interface_->robot_state_->getQActual(); + vel = robot_->rt_interface_->robot_state_->getQdActual(); + current = robot_->rt_interface_->robot_state_->getIActual(); + tcp = robot_->rt_interface_->robot_state_->getTcpForce(); + for (std::size_t i = 0; i < num_joints_; ++i) { + joint_position_[i] = pos[i]; + joint_velocity_[i] = vel[i]; + joint_effort_[i] = current[i]; + } + for (std::size_t i = 0; i < 3; ++i) { + robot_force_[i] = tcp[i]; + robot_torque_[i] = tcp[i + 3]; + } + +} + +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); +} + +} // namespace diff --git a/src/ur_ros_wrapper.cpp b/src/ur_ros_wrapper.cpp index e67c017..08edc72 100644 --- a/src/ur_ros_wrapper.cpp +++ b/src/ur_ros_wrapper.cpp @@ -10,6 +10,7 @@ */ #include "ur_modern_driver/ur_driver.h" +#include "ur_modern_driver/ur_hardware_interface.h" #include #include #include @@ -17,6 +18,7 @@ #include #include #include +#include #include "ros/ros.h" #include @@ -35,6 +37,7 @@ #include "ur_msgs/Digital.h" #include "ur_msgs/Analog.h" #include "std_msgs/String.h" +#include class RosWrapper { protected: @@ -52,9 +55,12 @@ protected: ros::ServiceServer payload_srv_; std::thread* rt_publish_thread_; std::thread* mb_publish_thread_; + std::thread* ros_control_thread_; double io_flag_delay_; double max_velocity_; std::vector joint_offsets_; + boost::shared_ptr hardware_interface_; + boost::shared_ptr controller_manager_; public: RosWrapper(std::string host) : @@ -76,6 +82,11 @@ public: joint_names.push_back(joint_prefix + "wrist_3_joint"); robot_.setJointNames(joint_names); + hardware_interface_.reset(new ros_control_ur::UrHardwareInterface(nh_, &robot_)); + controller_manager_.reset( + new controller_manager::ControllerManager( + hardware_interface_.get(), nh_)); + //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_)) { @@ -102,13 +113,13 @@ public: robot_.start(); //register the goal and feedback callbacks - as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this)); - as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this)); + //as_.registerGoalCallback(boost::bind(&RosWrapper::goalCB, this)); + //as_.registerPreemptCallback(boost::bind(&RosWrapper::preemptCB, this)); - as_.start(); + //as_.start(); //subscribe to the data topic of interest - speed_sub_ = nh_.subscribe("joint_speed", 1, + speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1, &RosWrapper::speedInterface, this); urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &RosWrapper::urscriptInterface, this); @@ -118,10 +129,12 @@ public: payload_srv_ = nh_.advertiseService("ur_driver/set_payload", &RosWrapper::setPayload, this); - rt_publish_thread_ = new std::thread( + ros_control_thread_ = new std::thread( + boost::bind(&RosWrapper::rosControlLoop, this)); + /*rt_publish_thread_ = new std::thread( boost::bind(&RosWrapper::publishRTMsg, this)); mb_publish_thread_ = new std::thread( - boost::bind(&RosWrapper::publishMbMsg, this)); + boost::bind(&RosWrapper::publishMbMsg, this)); */ ROS_DEBUG("The action server for this driver has been started"); } @@ -269,7 +282,7 @@ private: traj.points[i].velocities[mapping[j]]); if (traj.points[i].accelerations.size() != 0) new_point.accelerations.push_back( - traj.points[i].accelerations[mapping[j]]); + traj.points[i].accelerations[mapping[j]]); } new_point.time_from_start = traj.points[i].time_from_start; new_traj.push_back(new_point); @@ -330,18 +343,49 @@ private: } + void rosControlLoop() { + ros::Duration elapsed_time; + struct timespec last_time, current_time; + static const double BILLION = 1000000000.0; + + clock_gettime(CLOCK_MONOTONIC, &last_time); + while (ros::ok()) { + 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_->getControllerUpdated()) { + rt_msg_cond_.wait(locker); + } + clock_gettime(CLOCK_MONOTONIC, ¤t_time); + elapsed_time = ros::Duration( + current_time.tv_sec - last_time.tv_sec + + (current_time.tv_nsec - last_time.tv_nsec) + / BILLION); + last_time = current_time; + // Input + hardware_interface_->read(); + + // Control + //controller_manager_->update(ros::Time::now(), elapsed_time); + controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time); + + // Output + hardware_interface_->write(); + robot_.rt_interface_->robot_state_->setControllerUpdated(); + } + } + void publishRTMsg() { ros::Publisher joint_pub = nh_.advertise( - "/joint_states", 1); + "joint_states", 1); ros::Publisher wrench_pub = nh_.advertise( - "/wrench", 1); + "wrench", 1); while (ros::ok()) { sensor_msgs::JointState joint_msg; joint_msg.name = robot_.getJointNames(); geometry_msgs::WrenchStamped wrench_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_->getNewDataAvailable()) { + while (!robot_.rt_interface_->robot_state_->getDataPublished()) { rt_msg_cond_.wait(locker); } joint_msg.header.stamp = ros::Time::now(); @@ -365,13 +409,13 @@ private: wrench_msg.wrench.torque.z = tcp_force[5]; wrench_pub.publish(wrench_msg); - robot_.rt_interface_->robot_state_->finishedReading(); + robot_.rt_interface_->robot_state_->setDataPublished(); } } void publishMbMsg() { - ros::Publisher io_pub = nh_.advertise("/io_states", + ros::Publisher io_pub = nh_.advertise("ur_driver/io_states", 1); while (ros::ok()) { @@ -443,7 +487,11 @@ int main(int argc, char **argv) { RosWrapper interface(host); - ros::spin(); + ros::AsyncSpinner spinner(3); + spinner.start(); + + ros::waitForShutdown(); + interface.halt(); exit(0);