00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "ur_modern_driver/ur_driver.h"
00020 #include "ur_modern_driver/ur_hardware_interface.h"
00021 #include "ur_modern_driver/do_output.h"
00022 #include <string.h>
00023 #include <vector>
00024 #include <mutex>
00025 #include <condition_variable>
00026 #include <thread>
00027 #include <algorithm>
00028 #include <cmath>
00029 #include <chrono>
00030 #include <time.h>
00031
00032 #include "ros/ros.h"
00033 #include <ros/console.h>
00034 #include "sensor_msgs/JointState.h"
00035 #include "geometry_msgs/WrenchStamped.h"
00036 #include "geometry_msgs/PoseStamped.h"
00037 #include "control_msgs/FollowJointTrajectoryAction.h"
00038 #include "actionlib/server/action_server.h"
00039 #include "actionlib/server/server_goal_handle.h"
00040 #include "trajectory_msgs/JointTrajectoryPoint.h"
00041 #include "ur_msgs/SetIO.h"
00042 #include "ur_msgs/SetPayload.h"
00043 #include "ur_msgs/SetPayloadRequest.h"
00044 #include "ur_msgs/SetPayloadResponse.h"
00045 #include "ur_msgs/SetIORequest.h"
00046 #include "ur_msgs/SetIOResponse.h"
00047 #include "ur_msgs/IOStates.h"
00048 #include "ur_msgs/Digital.h"
00049 #include "ur_msgs/Analog.h"
00050 #include "std_msgs/String.h"
00051 #include <controller_manager/controller_manager.h>
00052 #include <realtime_tools/realtime_publisher.h>
00053
00055 #include <tf/tf.h>
00056 #include <tf/transform_broadcaster.h>
00057
00058 class RosWrapper {
00059 protected:
00060 UrDriver robot_;
00061 std::condition_variable rt_msg_cond_;
00062 std::condition_variable msg_cond_;
00063 ros::NodeHandle nh_;
00064 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00065 actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
00066 bool has_goal_;
00067 control_msgs::FollowJointTrajectoryFeedback feedback_;
00068 control_msgs::FollowJointTrajectoryResult result_;
00069 ros::Subscriber speed_sub_;
00070 ros::Subscriber urscript_sub_;
00071 ros::ServiceServer io_srv_;
00072 ros::ServiceServer payload_srv_;
00073 std::thread* rt_publish_thread_;
00074 std::thread* mb_publish_thread_;
00075 double io_flag_delay_;
00076 double max_velocity_;
00077 std::vector<double> joint_offsets_;
00078 std::string base_frame_;
00079 std::string tool_frame_;
00080 bool use_ros_control_;
00081 std::thread* ros_control_thread_;
00082 boost::shared_ptr<ros_control_ur::UrHardwareInterface> hardware_interface_;
00083 boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
00084
00085 public:
00086 RosWrapper(std::string host, int reverse_port) :
00087 as_(nh_, "follow_joint_trajectory",
00088 boost::bind(&RosWrapper::goalCB, this, _1),
00089 boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_(
00090 rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_(
00091 6, 0.0) {
00092
00093 std::string joint_prefix = "";
00094 std::vector<std::string> joint_names;
00095 char buf[256];
00096
00097 if (ros::param::get("~prefix", joint_prefix)) {
00098 if (joint_prefix.length() > 0) {
00099 sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
00100 print_info(buf);
00101 }
00102 }
00103 joint_names.push_back(joint_prefix + "shoulder_pan_joint");
00104 joint_names.push_back(joint_prefix + "shoulder_lift_joint");
00105 joint_names.push_back(joint_prefix + "elbow_joint");
00106 joint_names.push_back(joint_prefix + "wrist_1_joint");
00107 joint_names.push_back(joint_prefix + "wrist_2_joint");
00108 joint_names.push_back(joint_prefix + "wrist_3_joint");
00109 robot_.setJointNames(joint_names);
00110
00111 use_ros_control_ = false;
00112 ros::param::get("~use_ros_control", use_ros_control_);
00113
00114 if (use_ros_control_) {
00115 hardware_interface_.reset(
00116 new ros_control_ur::UrHardwareInterface(nh_, &robot_));
00117 controller_manager_.reset(
00118 new controller_manager::ControllerManager(
00119 hardware_interface_.get(), nh_));
00120 double max_vel_change = 0.12;
00121 if (ros::param::get("~max_acceleration", max_vel_change)) {
00122 max_vel_change = max_vel_change / 125;
00123 }
00124 sprintf(buf, "Max acceleration set to: %f [rad/sec²]",
00125 max_vel_change * 125);
00126 print_debug(buf);
00127 hardware_interface_->setMaxVelChange(max_vel_change);
00128 }
00129
00130 max_velocity_ = 10.;
00131 if (ros::param::get("~max_velocity", max_velocity_)) {
00132 sprintf(buf, "Max velocity accepted by ur_driver: %f [rad/s]",
00133 max_velocity_);
00134 print_debug(buf);
00135 }
00136
00137
00138
00139 double min_payload = 0.;
00140 double max_payload = 1.;
00141 if (ros::param::get("~min_payload", min_payload)) {
00142 sprintf(buf, "Min payload set to: %f [kg]", min_payload);
00143 print_debug(buf);
00144 }
00145 if (ros::param::get("~max_payload", max_payload)) {
00146 sprintf(buf, "Max payload set to: %f [kg]", max_payload);
00147 print_debug(buf);
00148 }
00149 robot_.setMinPayload(min_payload);
00150 robot_.setMaxPayload(max_payload);
00151 sprintf(buf, "Bounds for set_payload service calls: [%f, %f]",
00152 min_payload, max_payload);
00153 print_debug(buf);
00154
00155 double servoj_time = 0.008;
00156 if (ros::param::get("~servoj_time", servoj_time)) {
00157 sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
00158 print_debug(buf);
00159 }
00160 robot_.setServojTime(servoj_time);
00161
00162 double servoj_lookahead_time = 0.03;
00163 if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) {
00164 sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
00165 print_debug(buf);
00166 }
00167 robot_.setServojLookahead(servoj_lookahead_time);
00168
00169 double servoj_gain = 300.;
00170 if (ros::param::get("~servoj_gain", servoj_gain)) {
00171 sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
00172 print_debug(buf);
00173 }
00174 robot_.setServojGain(servoj_gain);
00175
00176
00177 base_frame_ = joint_prefix + "base_link";
00178 tool_frame_ = joint_prefix + "tool0_controller";
00179 if (ros::param::get("~base_frame", base_frame_)) {
00180 sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
00181 print_debug(buf);
00182 }
00183 if (ros::param::get("~tool_frame", tool_frame_)) {
00184 sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
00185 print_debug(buf);
00186 }
00187
00188 if (robot_.start()) {
00189 if (use_ros_control_) {
00190 ros_control_thread_ = new std::thread(
00191 boost::bind(&RosWrapper::rosControlLoop, this));
00192 print_debug(
00193 "The control thread for this driver has been started");
00194 } else {
00195
00196 has_goal_ = false;
00197 as_.start();
00198
00199
00200 rt_publish_thread_ = new std::thread(
00201 boost::bind(&RosWrapper::publishRTMsg, this));
00202 print_debug(
00203 "The action server for this driver has been started");
00204 }
00205 mb_publish_thread_ = new std::thread(
00206 boost::bind(&RosWrapper::publishMbMsg, this));
00207 speed_sub_ = nh_.subscribe("ur_driver/joint_speed", 1,
00208 &RosWrapper::speedInterface, this);
00209 urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1,
00210 &RosWrapper::urscriptInterface, this);
00211
00212 io_srv_ = nh_.advertiseService("ur_driver/set_io",
00213 &RosWrapper::setIO, this);
00214 payload_srv_ = nh_.advertiseService("ur_driver/set_payload",
00215 &RosWrapper::setPayload, this);
00216 }
00217 }
00218
00219 void halt() {
00220 robot_.halt();
00221 rt_publish_thread_->join();
00222
00223 }
00224 private:
00225 void trajThread(std::vector<double> timestamps,
00226 std::vector<std::vector<double> > positions,
00227 std::vector<std::vector<double> > velocities) {
00228
00229 robot_.doTraj(timestamps, positions, velocities);
00230 if (has_goal_) {
00231 result_.error_code = result_.SUCCESSFUL;
00232 goal_handle_.setSucceeded(result_);
00233 has_goal_ = false;
00234 }
00235 }
00236 void goalCB(
00237 actionlib::ServerGoalHandle<
00238 control_msgs::FollowJointTrajectoryAction> gh) {
00239 std::string buf;
00240 print_info("on_goal");
00241 if (!robot_.sec_interface_->robot_state_->isReady()) {
00242 result_.error_code = -100;
00243
00244 if (!robot_.sec_interface_->robot_state_->isPowerOnRobot()) {
00245 result_.error_string =
00246 "Cannot accept new trajectories: Robot arm is not powered on";
00247 gh.setRejected(result_, result_.error_string);
00248 print_error(result_.error_string);
00249 return;
00250 }
00251 if (!robot_.sec_interface_->robot_state_->isRealRobotEnabled()) {
00252 result_.error_string =
00253 "Cannot accept new trajectories: Robot is not enabled";
00254 gh.setRejected(result_, result_.error_string);
00255 print_error(result_.error_string);
00256 return;
00257 }
00258 result_.error_string =
00259 "Cannot accept new trajectories. (Debug: Robot mode is "
00260 + std::to_string(
00261 robot_.sec_interface_->robot_state_->getRobotMode())
00262 + ")";
00263 gh.setRejected(result_, result_.error_string);
00264 print_error(result_.error_string);
00265 return;
00266 }
00267 if (robot_.sec_interface_->robot_state_->isEmergencyStopped()) {
00268 result_.error_code = -100;
00269 result_.error_string =
00270 "Cannot accept new trajectories: Robot is emergency stopped";
00271 gh.setRejected(result_, result_.error_string);
00272 print_error(result_.error_string);
00273 return;
00274 }
00275 if (robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
00276 result_.error_code = -100;
00277 result_.error_string =
00278 "Cannot accept new trajectories: Robot is protective stopped";
00279 gh.setRejected(result_, result_.error_string);
00280 print_error(result_.error_string);
00281 return;
00282 }
00283
00284 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00285 *gh.getGoal();
00286 if (has_goal_) {
00287 print_warning(
00288 "Received new goal while still executing previous trajectory. Canceling previous trajectory");
00289 has_goal_ = false;
00290 robot_.stopTraj();
00291 result_.error_code = -100;
00292 result_.error_string = "Received another trajectory";
00293 goal_handle_.setAborted(result_, result_.error_string);
00294 std::this_thread::sleep_for(std::chrono::milliseconds(250));
00295 }
00296 goal_handle_ = gh;
00297 if (!validateJointNames()) {
00298 std::string outp_joint_names = "";
00299 for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
00300 i++) {
00301 outp_joint_names += goal.trajectory.joint_names[i] + " ";
00302 }
00303 result_.error_code = result_.INVALID_JOINTS;
00304 result_.error_string =
00305 "Received a goal with incorrect joint names: "
00306 + outp_joint_names;
00307 gh.setRejected(result_, result_.error_string);
00308 print_error(result_.error_string);
00309 return;
00310 }
00311 if (!has_positions()) {
00312 result_.error_code = result_.INVALID_GOAL;
00313 result_.error_string = "Received a goal without positions";
00314 gh.setRejected(result_, result_.error_string);
00315 print_error(result_.error_string);
00316 return;
00317 }
00318
00319 if (!has_velocities()) {
00320 result_.error_code = result_.INVALID_GOAL;
00321 result_.error_string = "Received a goal without velocities";
00322 gh.setRejected(result_, result_.error_string);
00323 print_error(result_.error_string);
00324 return;
00325 }
00326
00327 if (!traj_is_finite()) {
00328 result_.error_string = "Received a goal with infinities or NaNs";
00329 result_.error_code = result_.INVALID_GOAL;
00330 gh.setRejected(result_, result_.error_string);
00331 print_error(result_.error_string);
00332 return;
00333 }
00334
00335 if (!has_limited_velocities()) {
00336 result_.error_code = result_.INVALID_GOAL;
00337 result_.error_string =
00338 "Received a goal with velocities that are higher than "
00339 + std::to_string(max_velocity_);
00340 gh.setRejected(result_, result_.error_string);
00341 print_error(result_.error_string);
00342 return;
00343 }
00344
00345 reorder_traj_joints(goal.trajectory);
00346
00347 if (!start_positions_match(goal.trajectory, 0.01)) {
00348 result_.error_code = result_.INVALID_GOAL;
00349 result_.error_string = "Goal start doesn't match current pose";
00350 gh.setRejected(result_, result_.error_string);
00351 print_error(result_.error_string);
00352 return;
00353 }
00354
00355 std::vector<double> timestamps;
00356 std::vector<std::vector<double> > positions, velocities;
00357 if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
00358 print_warning(
00359 "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
00360 timestamps.push_back(0.0);
00361 positions.push_back(
00362 robot_.rt_interface_->robot_state_->getQActual());
00363 velocities.push_back(
00364 robot_.rt_interface_->robot_state_->getQdActual());
00365 }
00366 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00367 timestamps.push_back(
00368 goal.trajectory.points[i].time_from_start.toSec());
00369 positions.push_back(goal.trajectory.points[i].positions);
00370 velocities.push_back(goal.trajectory.points[i].velocities);
00371
00372 }
00373
00374 goal_handle_.setAccepted();
00375 has_goal_ = true;
00376 std::thread(&RosWrapper::trajThread, this, timestamps, positions,
00377 velocities).detach();
00378 }
00379
00380 void cancelCB(
00381 actionlib::ServerGoalHandle<
00382 control_msgs::FollowJointTrajectoryAction> gh) {
00383
00384 print_info("on_cancel");
00385 if (has_goal_) {
00386 if (gh == goal_handle_) {
00387 robot_.stopTraj();
00388 has_goal_ = false;
00389 }
00390 }
00391 result_.error_code = -100;
00392 result_.error_string = "Goal cancelled by client";
00393 gh.setCanceled(result_);
00394 }
00395
00396 bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) {
00397 resp.success = true;
00398
00399 if (req.fun == 1) {
00400 robot_.setDigitalOut(req.pin, req.state > 0.0 ? true : false);
00401 } else if (req.fun == 2) {
00402
00403 robot_.setFlag(req.pin, req.state > 0.0 ? true : false);
00404
00405 ros::Duration(io_flag_delay_).sleep();
00406 } else if (req.fun == 3) {
00407
00408 robot_.setAnalogOut(req.pin, req.state);
00409 } else if (req.fun == 4) {
00410
00411 robot_.setToolVoltage((int) req.state);
00412 } else {
00413 resp.success = false;
00414 }
00415 return resp.success;
00416 }
00417
00418 bool setPayload(ur_msgs::SetPayloadRequest& req,
00419 ur_msgs::SetPayloadResponse& resp) {
00420 if (robot_.setPayload(req.payload))
00421 resp.success = true;
00422 else
00423 resp.success = true;
00424 return resp.success;
00425 }
00426
00427 bool validateJointNames() {
00428 std::vector<std::string> actual_joint_names = robot_.getJointNames();
00429 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00430 *goal_handle_.getGoal();
00431 if (goal.trajectory.joint_names.size() != actual_joint_names.size())
00432 return false;
00433
00434 for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
00435 unsigned int j;
00436 for (j = 0; j < actual_joint_names.size(); j++) {
00437 if (goal.trajectory.joint_names[i] == actual_joint_names[j])
00438 break;
00439 }
00440 if (goal.trajectory.joint_names[i] == actual_joint_names[j]) {
00441 actual_joint_names.erase(actual_joint_names.begin() + j);
00442 } else {
00443 return false;
00444 }
00445 }
00446
00447 return true;
00448 }
00449
00450 void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) {
00451
00452 std::vector<std::string> actual_joint_names = robot_.getJointNames();
00453 std::vector<unsigned int> mapping;
00454 mapping.resize(actual_joint_names.size(), actual_joint_names.size());
00455 for (unsigned int i = 0; i < traj.joint_names.size(); i++) {
00456 for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
00457 if (traj.joint_names[i] == actual_joint_names[j])
00458 mapping[j] = i;
00459 }
00460 }
00461 traj.joint_names = actual_joint_names;
00462 std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
00463 for (unsigned int i = 0; i < traj.points.size(); i++) {
00464 trajectory_msgs::JointTrajectoryPoint new_point;
00465 for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) {
00466 new_point.positions.push_back(
00467 traj.points[i].positions[mapping[j]]);
00468 new_point.velocities.push_back(
00469 traj.points[i].velocities[mapping[j]]);
00470 if (traj.points[i].accelerations.size() != 0)
00471 new_point.accelerations.push_back(
00472 traj.points[i].accelerations[mapping[j]]);
00473 }
00474 new_point.time_from_start = traj.points[i].time_from_start;
00475 new_traj.push_back(new_point);
00476 }
00477 traj.points = new_traj;
00478 }
00479
00480 bool has_velocities() {
00481 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00482 *goal_handle_.getGoal();
00483 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00484 if (goal.trajectory.points[i].positions.size()
00485 != goal.trajectory.points[i].velocities.size())
00486 return false;
00487 }
00488 return true;
00489 }
00490
00491 bool has_positions() {
00492 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00493 *goal_handle_.getGoal();
00494 if (goal.trajectory.points.size() == 0)
00495 return false;
00496 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00497 if (goal.trajectory.points[i].positions.size()
00498 != goal.trajectory.joint_names.size())
00499 return false;
00500 }
00501 return true;
00502 }
00503
00504 bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps)
00505 {
00506 for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
00507 {
00508 std::vector<double> qActual = robot_.rt_interface_->robot_state_->getQActual();
00509 if( fabs(traj.points[0].positions[i] - qActual[i]) > eps )
00510 {
00511 return false;
00512 }
00513 }
00514 return true;
00515 }
00516
00517 bool has_limited_velocities() {
00518 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00519 *goal_handle_.getGoal();
00520 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00521 for (unsigned int j = 0;
00522 j < goal.trajectory.points[i].velocities.size(); j++) {
00523 if (fabs(goal.trajectory.points[i].velocities[j])
00524 > max_velocity_)
00525 return false;
00526 }
00527 }
00528 return true;
00529 }
00530
00531 bool traj_is_finite() {
00532 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00533 *goal_handle_.getGoal();
00534 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00535 for (unsigned int j = 0;
00536 j < goal.trajectory.points[i].velocities.size(); j++) {
00537 if (!std::isfinite(goal.trajectory.points[i].positions[j]))
00538 return false;
00539 if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
00540 return false;
00541 }
00542 }
00543 return true;
00544 }
00545
00546 void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) {
00547 if (msg->points[0].velocities.size() == 6) {
00548 double acc = 100;
00549 if (msg->points[0].accelerations.size() > 0)
00550 acc = *std::max_element(msg->points[0].accelerations.begin(),
00551 msg->points[0].accelerations.end());
00552 robot_.setSpeed(msg->points[0].velocities[0],
00553 msg->points[0].velocities[1], msg->points[0].velocities[2],
00554 msg->points[0].velocities[3], msg->points[0].velocities[4],
00555 msg->points[0].velocities[5], acc);
00556 }
00557
00558 }
00559 void urscriptInterface(const std_msgs::String::ConstPtr& msg) {
00560
00561 robot_.rt_interface_->addCommandToQueue(msg->data);
00562
00563 }
00564
00565 void rosControlLoop() {
00566 ros::Duration elapsed_time;
00567 struct timespec last_time, current_time;
00568 static const double BILLION = 1000000000.0;
00569
00570 realtime_tools::RealtimePublisher<tf::tfMessage> tf_pub( nh_, "/tf", 1 );
00571 geometry_msgs::TransformStamped tool_transform;
00572 tool_transform.header.frame_id = base_frame_;
00573 tool_transform.child_frame_id = tool_frame_;
00574 tf_pub.msg_.transforms.push_back( tool_transform );
00575
00576 realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> tool_vel_pub( nh_, "tool_velocity", 1 );
00577 tool_vel_pub.msg_.header.frame_id = base_frame_;
00578
00579
00580 clock_gettime(CLOCK_MONOTONIC, &last_time);
00581 while (ros::ok()) {
00582 std::mutex msg_lock;
00583 std::unique_lock<std::mutex> locker(msg_lock);
00584 while (!robot_.rt_interface_->robot_state_->getControllerUpdated()) {
00585 rt_msg_cond_.wait(locker);
00586 }
00587
00588 hardware_interface_->read();
00589 robot_.rt_interface_->robot_state_->setControllerUpdated();
00590
00591
00592 clock_gettime(CLOCK_MONOTONIC, ¤t_time);
00593 elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION);
00594 ros::Time ros_time = ros::Time::now();
00595 controller_manager_->update(ros_time, elapsed_time);
00596 last_time = current_time;
00597
00598
00599 hardware_interface_->write();
00600
00601
00602 std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
00603
00604
00605 double rx = tool_vector_actual[3];
00606 double ry = tool_vector_actual[4];
00607 double rz = tool_vector_actual[5];
00608 double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
00609
00610
00611 if( tf_pub.trylock() )
00612 {
00613 tf_pub.msg_.transforms[0].header.stamp = ros_time;
00614 if (angle < 1e-16) {
00615 tf_pub.msg_.transforms[0].transform.rotation.x = 0;
00616 tf_pub.msg_.transforms[0].transform.rotation.y = 0;
00617 tf_pub.msg_.transforms[0].transform.rotation.z = 0;
00618 tf_pub.msg_.transforms[0].transform.rotation.w = 1;
00619 } else {
00620 tf_pub.msg_.transforms[0].transform.rotation.x = (rx/angle) * std::sin(angle*0.5);
00621 tf_pub.msg_.transforms[0].transform.rotation.y = (ry/angle) * std::sin(angle*0.5);
00622 tf_pub.msg_.transforms[0].transform.rotation.z = (rz/angle) * std::sin(angle*0.5);
00623 tf_pub.msg_.transforms[0].transform.rotation.w = std::cos(angle*0.5);
00624 }
00625 tf_pub.msg_.transforms[0].transform.translation.x = tool_vector_actual[0];
00626 tf_pub.msg_.transforms[0].transform.translation.y = tool_vector_actual[1];
00627 tf_pub.msg_.transforms[0].transform.translation.z = tool_vector_actual[2];
00628
00629 tf_pub.unlockAndPublish();
00630 }
00631
00632
00633 std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
00634
00635 if( tool_vel_pub.trylock() )
00636 {
00637 tool_vel_pub.msg_.header.stamp = ros_time;
00638 tool_vel_pub.msg_.twist.linear.x = tcp_speed[0];
00639 tool_vel_pub.msg_.twist.linear.y = tcp_speed[1];
00640 tool_vel_pub.msg_.twist.linear.z = tcp_speed[2];
00641 tool_vel_pub.msg_.twist.angular.x = tcp_speed[3];
00642 tool_vel_pub.msg_.twist.angular.y = tcp_speed[4];
00643 tool_vel_pub.msg_.twist.angular.z = tcp_speed[5];
00644
00645 tool_vel_pub.unlockAndPublish();
00646 }
00647
00648 }
00649 }
00650
00651 void publishRTMsg() {
00652 ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>(
00653 "joint_states", 1);
00654 ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>(
00655 "wrench", 1);
00656 ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
00657 static tf::TransformBroadcaster br;
00658 while (ros::ok()) {
00659 sensor_msgs::JointState joint_msg;
00660 joint_msg.name = robot_.getJointNames();
00661 geometry_msgs::WrenchStamped wrench_msg;
00662 geometry_msgs::PoseStamped tool_pose_msg;
00663 std::mutex msg_lock;
00664 std::unique_lock<std::mutex> locker(msg_lock);
00665 while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
00666 rt_msg_cond_.wait(locker);
00667 }
00668 joint_msg.header.stamp = ros::Time::now();
00669 joint_msg.position =
00670 robot_.rt_interface_->robot_state_->getQActual();
00671 for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
00672 joint_msg.position[i] += joint_offsets_[i];
00673 }
00674 joint_msg.velocity =
00675 robot_.rt_interface_->robot_state_->getQdActual();
00676 joint_msg.effort = robot_.rt_interface_->robot_state_->getIActual();
00677 joint_pub.publish(joint_msg);
00678 std::vector<double> tcp_force =
00679 robot_.rt_interface_->robot_state_->getTcpForce();
00680 wrench_msg.header.stamp = joint_msg.header.stamp;
00681 wrench_msg.wrench.force.x = tcp_force[0];
00682 wrench_msg.wrench.force.y = tcp_force[1];
00683 wrench_msg.wrench.force.z = tcp_force[2];
00684 wrench_msg.wrench.torque.x = tcp_force[3];
00685 wrench_msg.wrench.torque.y = tcp_force[4];
00686 wrench_msg.wrench.torque.z = tcp_force[5];
00687 wrench_pub.publish(wrench_msg);
00688
00689
00690 std::vector<double> tool_vector_actual = robot_.rt_interface_->robot_state_->getToolVectorActual();
00691
00692
00693 tf::Quaternion quat;
00694 double rx = tool_vector_actual[3];
00695 double ry = tool_vector_actual[4];
00696 double rz = tool_vector_actual[5];
00697 double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
00698 if (angle < 1e-16) {
00699 quat.setValue(0, 0, 0, 1);
00700 } else {
00701 quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
00702 }
00703
00704
00705 tf::Transform transform;
00706 transform.setOrigin(tf::Vector3(tool_vector_actual[0], tool_vector_actual[1], tool_vector_actual[2]));
00707 transform.setRotation(quat);
00708 br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
00709
00710
00711 std::vector<double> tcp_speed =
00712 robot_.rt_interface_->robot_state_->getTcpSpeedActual();
00713 geometry_msgs::TwistStamped tool_twist;
00714 tool_twist.header.frame_id = base_frame_;
00715 tool_twist.header.stamp = joint_msg.header.stamp;
00716 tool_twist.twist.linear.x = tcp_speed[0];
00717 tool_twist.twist.linear.y = tcp_speed[1];
00718 tool_twist.twist.linear.z = tcp_speed[2];
00719 tool_twist.twist.angular.x = tcp_speed[3];
00720 tool_twist.twist.angular.y = tcp_speed[4];
00721 tool_twist.twist.angular.z = tcp_speed[5];
00722 tool_vel_pub.publish(tool_twist);
00723
00724 robot_.rt_interface_->robot_state_->setDataPublished();
00725 }
00726 }
00727
00728 void publishMbMsg() {
00729 bool warned = false;
00730 ros::Publisher io_pub = nh_.advertise<ur_msgs::IOStates>(
00731 "ur_driver/io_states", 1);
00732
00733 while (ros::ok()) {
00734 ur_msgs::IOStates io_msg;
00735 std::mutex msg_lock;
00736 std::unique_lock<std::mutex> locker(msg_lock);
00737 while (!robot_.sec_interface_->robot_state_->getNewDataAvailable()) {
00738 msg_cond_.wait(locker);
00739 }
00740 int i_max = 10;
00741 if (robot_.sec_interface_->robot_state_->getVersion() > 3.0)
00742 i_max = 18;
00743 for (unsigned int i = 0; i < i_max; i++) {
00744 ur_msgs::Digital digi;
00745 digi.pin = i;
00746 digi.state =
00747 ((robot_.sec_interface_->robot_state_->getDigitalInputBits()
00748 & (1 << i)) >> i);
00749 io_msg.digital_in_states.push_back(digi);
00750 digi.state =
00751 ((robot_.sec_interface_->robot_state_->getDigitalOutputBits()
00752 & (1 << i)) >> i);
00753 io_msg.digital_out_states.push_back(digi);
00754 }
00755 ur_msgs::Analog ana;
00756 ana.pin = 0;
00757 ana.state = robot_.sec_interface_->robot_state_->getAnalogInput0();
00758 io_msg.analog_in_states.push_back(ana);
00759 ana.pin = 1;
00760 ana.state = robot_.sec_interface_->robot_state_->getAnalogInput1();
00761 io_msg.analog_in_states.push_back(ana);
00762
00763 ana.pin = 0;
00764 ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput0();
00765 io_msg.analog_out_states.push_back(ana);
00766 ana.pin = 1;
00767 ana.state = robot_.sec_interface_->robot_state_->getAnalogOutput1();
00768 io_msg.analog_out_states.push_back(ana);
00769 io_pub.publish(io_msg);
00770
00771 if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
00772 or robot_.sec_interface_->robot_state_->isProtectiveStopped()) {
00773 if (robot_.sec_interface_->robot_state_->isEmergencyStopped()
00774 and !warned) {
00775 print_error("Emergency stop pressed!");
00776 } else if (robot_.sec_interface_->robot_state_->isProtectiveStopped()
00777 and !warned) {
00778 print_error("Robot is protective stopped!");
00779 }
00780 if (has_goal_) {
00781 print_error("Aborting trajectory");
00782 robot_.stopTraj();
00783 result_.error_code = result_.SUCCESSFUL;
00784 result_.error_string = "Robot was halted";
00785 goal_handle_.setAborted(result_, result_.error_string);
00786 has_goal_ = false;
00787 }
00788 warned = true;
00789 } else
00790 warned = false;
00791
00792 robot_.sec_interface_->robot_state_->finishedReading();
00793
00794 }
00795 }
00796
00797 };
00798
00799 int main(int argc, char **argv) {
00800 bool use_sim_time = false;
00801 std::string host;
00802 int reverse_port = 50001;
00803
00804 ros::init(argc, argv, "ur_driver");
00805 ros::NodeHandle nh;
00806 if (ros::param::get("use_sim_time", use_sim_time)) {
00807 print_warning("use_sim_time is set!!");
00808 }
00809 if (!(ros::param::get("~robot_ip_address", host))) {
00810 if (argc > 1) {
00811 print_warning(
00812 "Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED");
00813 host = argv[1];
00814 } else {
00815 print_fatal(
00816 "Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
00817 exit(1);
00818 }
00819
00820 }
00821 if ((ros::param::get("~reverse_port", reverse_port))) {
00822 if((reverse_port <= 0) or (reverse_port >= 65535)) {
00823 print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 50001");
00824 reverse_port = 50001;
00825 }
00826 } else
00827 reverse_port = 50001;
00828
00829 RosWrapper interface(host, reverse_port);
00830
00831 ros::AsyncSpinner spinner(3);
00832 spinner.start();
00833
00834 ros::waitForShutdown();
00835
00836 interface.halt();
00837
00838 exit(0);
00839 }