Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Implemented ros-control

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-09-24 10:27:29 +02:00
parent d5ea350145
commit af601b9c32
12 changed files with 549 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
CMakeLists.txt
View File

@@ -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}
)
#############

55
config/ur10_controllers.yaml Normal file
View File

@@ -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

60
config/ur5_controllers.yaml Normal file
View File

@@ -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

8
include/ur_modern_driver/robot_state_RT.h
View File

@@ -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<double> 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<double> getVActual();
void unpack(uint8_t * buf);
};

117
include/ur_modern_driver/ur_hardware_interface.h Normal file
View File

@@ -0,0 +1,117 @@
/*
* ur_hardware_control_loop.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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 <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/robot_hw.h>
#include <controller_manager/controller_manager.h>
#include <boost/scoped_ptr.hpp>
#include <ros/ros.h>
#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<std::string> joint_names_;
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> 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

40
launch/ur10_ros_control.launch Normal file
View File

@@ -0,0 +1,40 @@
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<group ns="universal_robot">
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
<param name="min_payload" type="double" value="$(arg min_payload)"/>
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
</node>
<!-- Load controller settings -->
<rosparam file="$(find ur_modern_driver)/config/ur10_controllers.yaml" command="load"/>
<!-- Load controller manager -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</group>
</launch>

2
launch/ur5_bringup.launch
View File

@@ -12,7 +12,7 @@
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="10.0"/>
<arg name="max_payload" default="5.0"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">

40
launch/ur5_ros_control.launch Normal file
View File

@@ -0,0 +1,40 @@
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="min_payload" default="0.0"/>
<arg name="max_payload" default="5.0"/>
<arg name="max_velocity" default="10.0"/> <!-- [rad/s] -->
<!-- robot model -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<group ns="universal_robot">
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
<param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
<param name="min_payload" type="double" value="$(arg min_payload)"/>
<param name="max_payload" type="double" value="$(arg max_payload)"/>
<param name="max_velocity" type="double" value="$(arg max_velocity)"/>
</node>
<!-- Load controller settings -->
<rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
<!-- Load controller manager -->
<node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" />
<!-- Convert joint states to /tf tranforms -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</group>
</launch>

6
package.xml
View File

@@ -40,6 +40,8 @@
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>hardware_interface</build_depend>
<build_depend>controller_manager</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
@@ -48,6 +50,9 @@
<build_depend>std_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>ur_msgs</build_depend>
<run_depend>hardware_interface</run_depend>
<run_depend>controller_manager</run_depend>
<run_depend>ros_controllers</run_depend>
<run_depend>actionlib</run_depend>
<run_depend>control_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
@@ -56,6 +61,7 @@
<run_depend>std_msgs</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>ur_msgs</run_depend>
<run_depend>ur_description</run_depend>
<!-- The export tag contains other, unspecified, tags -->

25
src/robot_state_RT.cpp
View File

@@ -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();
}

133
src/ur_hardware_interface.cpp Normal file
View File

@@ -0,0 +1,133 @@
/*
* ur_hardware_control_loop.cpp
*
* ----------------------------------------------------------------------------
* "THE BEER-WARE LICENSE" (Revision 42):
* <thomas.timm.dk@gmail.com> 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 <ur_modern_driver/ur_hardware_interface.h>
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<double> 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

74
src/ur_ros_wrapper.cpp
View File

@@ -10,6 +10,7 @@
*/
#include "ur_modern_driver/ur_driver.h"
#include "ur_modern_driver/ur_hardware_interface.h"
#include <string.h>
#include <vector>
#include <mutex>
@@ -17,6 +18,7 @@
#include <thread>
#include <algorithm>
#include <cmath>
#include <time.h>
#include "ros/ros.h"
#include <ros/console.h>
@@ -35,6 +37,7 @@
#include "ur_msgs/Digital.h"
#include "ur_msgs/Analog.h"
#include "std_msgs/String.h"
#include <controller_manager/controller_manager.h>
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<double> joint_offsets_;
boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
boost::shared_ptr<controller_manager::ControllerManager> 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<std::mutex> locker(msg_lock);
while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) {
rt_msg_cond_.wait(locker);
}
clock_gettime(CLOCK_MONOTONIC, &current_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<sensor_msgs::JointState>(
"/joint_states", 1);
"joint_states", 1);
ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
"/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<std::mutex> 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<ur_msgs::IOStates>("/io_states",
ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>("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);