aubo_ros_wrapper.cpp
Go to the documentation of this file.
00001 #include "aubo_new_driver/aubo_new_driver.h"
00002 #include "aubo_new_driver/aubo_hardware_interface.h"
00003 #include "aubo_new_driver/do_output.h"
00004 #include <string.h>
00005 #include <vector>
00006 #include <mutex>
00007 #include <condition_variable>
00008 #include <thread>
00009 #include <algorithm>
00010 #include <cmath>
00011 #include <chrono>
00012 #include <time.h>
00013 
00014 #include "ros/ros.h"
00015 #include <ros/console.h>
00016 #include "sensor_msgs/JointState.h"
00017 #include "geometry_msgs/WrenchStamped.h"
00018 #include "geometry_msgs/PoseStamped.h"
00019 #include "control_msgs/FollowJointTrajectoryAction.h"
00020 #include "actionlib/server/action_server.h"
00021 #include "actionlib/server/server_goal_handle.h"
00022 #include "trajectory_msgs/JointTrajectoryPoint.h"
00023 
00024 
00025 #include "aubo_msgs/SetIO.h"
00026 #include "aubo_msgs/SetIORequest.h"
00027 #include "aubo_msgs/SetIOResponse.h"
00028 
00029 #include "std_msgs/String.h"
00030 #include "std_msgs/Float32MultiArray.h"
00031 #include <controller_manager/controller_manager.h>
00032 
00034 #include <tf/tf.h>
00035 #include <tf/transform_broadcaster.h>
00036 
00037 class RosWrapper {
00038 protected:
00039         AuboNewDriver robot_;
00040         std::condition_variable rt_msg_cond_;
00041         std::condition_variable msg_cond_;
00042         ros::NodeHandle nh_;
00043         actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00044         actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> goal_handle_;
00045         bool has_goal_;
00046         control_msgs::FollowJointTrajectoryFeedback feedback_;
00047         control_msgs::FollowJointTrajectoryResult result_;
00048         ros::Subscriber speed_sub_;
00049     ros::Subscriber script_sub_;
00050     ros::Subscriber move_sub_;
00051         ros::ServiceServer io_srv_;
00052         std::thread* rt_publish_thread_;
00053         double io_flag_delay_;
00054         double max_velocity_;
00055         std::vector<double> joint_offsets_;
00056     std::string base_frame_;
00057     std::string tool_frame_;
00058         bool use_ros_control_;
00059         std::thread* ros_control_thread_;
00060         boost::shared_ptr<ros_control_aubo::AuboHardwareInterface> hardware_interface_;
00061         boost::shared_ptr<controller_manager::ControllerManager> controller_manager_;
00062 
00063 public:
00064         RosWrapper(std::string host, int reverse_port) :
00065             as_(nh_, "follow_joint_trajectory",
00066                                         boost::bind(&RosWrapper::goalCB, this, _1),
00067                                         boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_(
00068                                         rt_msg_cond_, msg_cond_, host, reverse_port), io_flag_delay_(0.05), joint_offsets_(
00069                                         6, 0.0) {
00070 
00071 
00072                 print_debug("The action server for this driver has been started");
00073 
00074                 std::string joint_prefix = "";
00075                 std::vector<std::string> joint_names;
00076                 char buf[256];
00077 
00078                 if (ros::param::get("~prefix", joint_prefix)) {
00079                     if (joint_prefix.length() > 0) {
00080                         sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
00081                         print_info(buf);
00082                     }   
00083                 }
00084 
00085                 joint_names.push_back(joint_prefix + "shoulder_joint");
00086                 joint_names.push_back(joint_prefix + "upperArm_joint");
00087                 joint_names.push_back(joint_prefix + "foreArm_joint");
00088                 joint_names.push_back(joint_prefix + "wrist1_joint");
00089                 joint_names.push_back(joint_prefix + "wrist2_joint");
00090                 joint_names.push_back(joint_prefix + "wrist3_joint");
00091                 robot_.setJointNames(joint_names);
00092 
00093                 use_ros_control_ = false;
00094                 ros::param::get("~use_ros_control", use_ros_control_);
00095 
00096 
00097                 if (use_ros_control_) {
00098                         hardware_interface_.reset(
00099                                         new ros_control_aubo::AuboHardwareInterface(nh_, &robot_));
00100                         controller_manager_.reset(
00101                                         new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
00102 
00103                         double max_vel_change = 0.06; // equivalent of an acceleration of 15 rad/sec^2
00104                         if (ros::param::get("~max_acceleration", max_vel_change)) {
00105                                 max_vel_change = max_vel_change / 125;
00106                         }
00107                         sprintf(buf, "Max acceleration set to: %f [rad/secĀ²]",
00108                                         max_vel_change * 125);
00109                         print_debug(buf);
00110                         hardware_interface_->setMaxVelChange(max_vel_change);
00111                 }
00112 
00113                 //Using a very high value in order to not limit execution of trajectories being sent from MoveIt!
00114                 max_velocity_ = 5.;
00115                 if (ros::param::get("~max_velocity", max_velocity_)) {
00116                         sprintf(buf, "Max velocity accepted by aubo_new_driver: %f [rad/s]",
00117                                         max_velocity_);
00118                         print_debug(buf);
00119                 }
00120 
00121 
00122         double servoj_time = 0.008;
00123         if (ros::param::get("~servoj_time", servoj_time)) {
00124             sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
00125             print_debug(buf);
00126         }
00127         robot_.setServojTime(servoj_time);
00128 
00129 
00130                 //Base and tool frames
00131         base_frame_ = joint_prefix + "base_Link";
00132                 tool_frame_ =  joint_prefix + "tool0_controller";
00133                 if (ros::param::get("~base_frame", base_frame_)) {
00134                     base_frame_ = base_frame_;
00135                     sprintf(buf, "Base frame set to: %s", base_frame_.c_str());
00136                     print_debug(buf);
00137                 }
00138                 if (ros::param::get("~tool_frame", tool_frame_)) {
00139                     sprintf(buf, "Tool frame set to: %s", tool_frame_.c_str());
00140                     print_debug(buf);
00141         }
00142 
00143                 if (robot_.start()) {
00144                         if (use_ros_control_) {
00145                                 ros_control_thread_ = new std::thread(boost::bind(&RosWrapper::rosControlLoop, this));
00146                                 print_debug("The control thread for this driver has been started");
00147                         } else {
00148                                 //start actionserver
00149                                 has_goal_ = false;
00150                                 as_.start();
00151 
00152                                 //subscribe to the data topic of interest
00153                                 rt_publish_thread_ = new std::thread(boost::bind(&RosWrapper::publishRTMsg, this));
00154                                 print_debug("The action server for this driver has been started");
00155                         }
00156 
00157                         speed_sub_ = nh_.subscribe("aubo_new_driver/joint_speed", 1,
00158                                         &RosWrapper::speedInterface, this);
00159 
00160             script_sub_ = nh_.subscribe("aubo_new_driver/aubo_script", 1,
00161                     &RosWrapper::scriptInterface, this);
00162 
00163             move_sub_ = nh_.subscribe("movej_cmd", 1,
00164                     &RosWrapper::moveInterface, this);
00165 
00166             io_srv_ = nh_.advertiseService("aubo_new_driver/set_io",
00167                     &RosWrapper::setIO, this);
00168                 }
00169         }
00170 
00171         void halt() {
00172                 robot_.halt();
00173                 rt_publish_thread_->join();
00174 
00175         }
00176 private:
00177         void trajThread(std::vector<double> timestamps,
00178                         std::vector<std::vector<double> > positions,
00179                         std::vector<std::vector<double> > velocities) {
00180 
00181                 robot_.doTraj(timestamps, positions, velocities);
00182                 if (has_goal_) {
00183                         result_.error_code = result_.SUCCESSFUL;
00184                         goal_handle_.setSucceeded(result_);
00185                         has_goal_ = false;
00186                 }
00187         }
00188         void goalCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh) {
00189                 std::string buf;
00190                 print_info("on_goal");
00191 
00192                 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00193                                 *gh.getGoal(); //make a copy that we can modify
00194                 if (has_goal_) {
00195                         print_warning(
00196                                         "Received new goal while still executing previous trajectory. Canceling previous trajectory");
00197                         has_goal_ = false;
00198                         robot_.stopTraj();
00199                         result_.error_code = -100; //nothing is defined for this...?
00200                         result_.error_string = "Received another trajectory";
00201                         goal_handle_.setAborted(result_, result_.error_string);
00202                         std::this_thread::sleep_for(std::chrono::milliseconds(250));
00203                 }
00204                 goal_handle_ = gh;
00205                 if (!validateJointNames()) {
00206                         std::string outp_joint_names = "";
00207                         for (unsigned int i = 0; i < goal.trajectory.joint_names.size();
00208                                         i++) {
00209                                 outp_joint_names += goal.trajectory.joint_names[i] + " ";
00210                         }
00211                         result_.error_code = result_.INVALID_JOINTS;
00212                         result_.error_string =
00213                                         "Received a goal with incorrect joint names: "
00214                                                         + outp_joint_names;
00215                         gh.setRejected(result_, result_.error_string);
00216                         print_error(result_.error_string);
00217                         return;
00218                 }
00219                 if (!has_positions()) {
00220                         result_.error_code = result_.INVALID_GOAL;
00221                         result_.error_string = "Received a goal without positions";
00222                         gh.setRejected(result_, result_.error_string);
00223                         print_error(result_.error_string);
00224                         return;
00225                 }
00226         
00227                 if (!has_velocities()) {
00228                         result_.error_code = result_.INVALID_GOAL;
00229                         result_.error_string = "Received a goal without velocities";
00230                         gh.setRejected(result_, result_.error_string);
00231                         print_error(result_.error_string);
00232                         return;
00233                 }
00234 
00235                 if (!traj_is_finite()) {
00236                         result_.error_string = "Received a goal with infinities or NaNs";
00237                         result_.error_code = result_.INVALID_GOAL;
00238                         gh.setRejected(result_, result_.error_string);
00239                         print_error(result_.error_string);
00240                         return;
00241                 }
00242 
00243                 if (!has_limited_velocities()) {
00244                         result_.error_code = result_.INVALID_GOAL;
00245                         result_.error_string =
00246                                         "Received a goal with velocities that are higher than "
00247                                                         + std::to_string(max_velocity_);
00248                         gh.setRejected(result_, result_.error_string);
00249                         print_error(result_.error_string);
00250                         return;
00251                 }
00252 
00253                 reorder_traj_joints(goal.trajectory);
00254                 
00255                 if (!start_positions_match(goal.trajectory, 0.01)) {
00256                         result_.error_code = result_.INVALID_GOAL;
00257                         result_.error_string = "Goal start doesn't match current pose";
00258                         gh.setRejected(result_, result_.error_string);
00259                         print_error(result_.error_string);
00260                         return;
00261                 }
00262 
00263                 std::vector<double> timestamps;
00264                 std::vector<std::vector<double> > positions, velocities;
00265                 if (goal.trajectory.points[0].time_from_start.toSec() != 0.) {
00266                         print_warning(
00267                                         "Trajectory's first point should be the current position, with time_from_start set to 0.0 - Inserting point in malformed trajectory");
00268                         timestamps.push_back(0.0);
00269                         positions.push_back(
00270                     robot_.rt_interface_->robot_state_->getJonitPosition());
00271                         velocities.push_back(
00272                     robot_.rt_interface_->robot_state_->getJonitVelocity());
00273                 }
00274                 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00275                         timestamps.push_back(
00276                                         goal.trajectory.points[i].time_from_start.toSec());
00277                         positions.push_back(goal.trajectory.points[i].positions);
00278                         velocities.push_back(goal.trajectory.points[i].velocities);
00279 
00280                 }
00281 
00282                 goal_handle_.setAccepted();
00283                 has_goal_ = true;
00284                 std::thread(&RosWrapper::trajThread, this, timestamps, positions,
00285                                 velocities).detach();
00286         }
00287 
00288         void cancelCB(actionlib::ServerGoalHandle<control_msgs::FollowJointTrajectoryAction> gh) {
00289                 // set the action state to preempted
00290                 print_info("on_cancel");
00291                 if (has_goal_) {
00292                         if (gh == goal_handle_) {
00293                                 robot_.stopTraj();
00294                                 has_goal_ = false;
00295                         }
00296                 }
00297                 result_.error_code = -100; //nothing is defined for this...?
00298                 result_.error_string = "Goal cancelled by client";
00299                 gh.setCanceled(result_);
00300         }
00301 
00302 
00303     bool setIO(aubo_msgs::SetIORequest& req, aubo_msgs::SetIOResponse& resp) {
00304         resp.success = true;
00305 
00306         robot_.setRobotIO(req.type,req.mode,req.index,req.state);
00307 
00308         ros::Duration(io_flag_delay_).sleep();
00309 
00310         return resp.success;
00311     }
00312 
00313         bool validateJointNames() {
00314                 std::vector<std::string> actual_joint_names = robot_.getJointNames();
00315                 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00316                                 *goal_handle_.getGoal();
00317                 if (goal.trajectory.joint_names.size() != actual_joint_names.size())
00318                         return false;
00319 
00320                 for (unsigned int i = 0; i < goal.trajectory.joint_names.size(); i++) {
00321                         unsigned int j;
00322                         for (j = 0; j < actual_joint_names.size(); j++) {
00323                                 if (goal.trajectory.joint_names[i] == actual_joint_names[j])
00324                                         break;
00325                         }
00326                         if (goal.trajectory.joint_names[i] == actual_joint_names[j]) {
00327                                 actual_joint_names.erase(actual_joint_names.begin() + j);
00328                         } else {
00329                                 return false;
00330                         }
00331                 }
00332 
00333                 return true;
00334         }
00335 
00336         void reorder_traj_joints(trajectory_msgs::JointTrajectory& traj) {
00337                 /* Reorders trajectory - destructive */
00338                 std::vector<std::string> actual_joint_names = robot_.getJointNames();
00339                 std::vector<unsigned int> mapping;
00340                 mapping.resize(actual_joint_names.size(), actual_joint_names.size());
00341                 for (unsigned int i = 0; i < traj.joint_names.size(); i++) {
00342                         for (unsigned int j = 0; j < actual_joint_names.size(); j++) {
00343                                 if (traj.joint_names[i] == actual_joint_names[j])
00344                                         mapping[j] = i;
00345                         }
00346                 }
00347                 traj.joint_names = actual_joint_names;
00348                 std::vector<trajectory_msgs::JointTrajectoryPoint> new_traj;
00349                 for (unsigned int i = 0; i < traj.points.size(); i++) {
00350                         trajectory_msgs::JointTrajectoryPoint new_point;
00351                         for (unsigned int j = 0; j < traj.points[i].positions.size(); j++) {
00352                                 new_point.positions.push_back(
00353                                                 traj.points[i].positions[mapping[j]]);
00354                                 new_point.velocities.push_back(
00355                                                 traj.points[i].velocities[mapping[j]]);
00356                                 if (traj.points[i].accelerations.size() != 0)
00357                                         new_point.accelerations.push_back(
00358                                                         traj.points[i].accelerations[mapping[j]]);
00359                         }
00360                         new_point.time_from_start = traj.points[i].time_from_start;
00361                         new_traj.push_back(new_point);
00362                 }
00363                 traj.points = new_traj;
00364         }
00365 
00366         bool has_velocities() {
00367                 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00368                                 *goal_handle_.getGoal();
00369                 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00370                         if (goal.trajectory.points[i].positions.size()
00371                                         != goal.trajectory.points[i].velocities.size())
00372                                 return false;
00373                 }
00374                 return true;
00375         }
00376 
00377         bool has_positions() {
00378                 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00379                                 *goal_handle_.getGoal();
00380                 if (goal.trajectory.points.size() == 0)
00381                         return false;
00382                 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00383                         if (goal.trajectory.points[i].positions.size()
00384                                         != goal.trajectory.joint_names.size())
00385                                 return false;
00386                 }
00387                 return true;
00388         }
00389 
00390         bool start_positions_match(const trajectory_msgs::JointTrajectory &traj, double eps)
00391         {
00392                 for (unsigned int i = 0; i < traj.points[0].positions.size(); i++)
00393                 {
00394             std::vector<double> qActual = robot_.rt_interface_->robot_state_->getJonitPosition();
00395                         if( fabs(traj.points[0].positions[i] - qActual[i]) > eps )
00396                         {
00397                                 return false;
00398                         }
00399                 }
00400                 return true;
00401         }
00402 
00403         bool has_limited_velocities() {
00404                 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00405                                 *goal_handle_.getGoal();
00406                 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00407                         for (unsigned int j = 0;
00408                                         j < goal.trajectory.points[i].velocities.size(); j++) {
00409                                 if (fabs(goal.trajectory.points[i].velocities[j])
00410                                                 > max_velocity_)
00411                                         return false;
00412                         }
00413                 }
00414                 return true;
00415         }
00416 
00417         bool traj_is_finite() {
00418                 actionlib::ActionServer<control_msgs::FollowJointTrajectoryAction>::Goal goal =
00419                                 *goal_handle_.getGoal();
00420                 for (unsigned int i = 0; i < goal.trajectory.points.size(); i++) {
00421                         for (unsigned int j = 0;
00422                                         j < goal.trajectory.points[i].velocities.size(); j++) {
00423                                 if (!std::isfinite(goal.trajectory.points[i].positions[j]))
00424                                         return false;
00425                                 if (!std::isfinite(goal.trajectory.points[i].velocities[j]))
00426                                         return false;
00427                         }
00428                 }
00429                 return true;
00430         }
00431 
00432         void speedInterface(const trajectory_msgs::JointTrajectory::Ptr& msg) {
00433                 if (msg->points[0].velocities.size() == 6) {
00434                         double acc = 100;
00435                         if (msg->points[0].accelerations.size() > 0)
00436                                 acc = *std::max_element(msg->points[0].accelerations.begin(),
00437                                                 msg->points[0].accelerations.end());
00438 
00439                         robot_.setSpeed(msg->points[0].velocities[0],
00440                                         msg->points[0].velocities[1], msg->points[0].velocities[2],
00441                                         msg->points[0].velocities[3], msg->points[0].velocities[4],
00442                                         msg->points[0].velocities[5], acc);
00443 
00444                 }
00445 
00446         }
00447 
00448     void scriptInterface(const std_msgs::String::ConstPtr& msg) {
00449 
00450         //not implement yet
00451         //robot_.rt_interface_->addCommandToQueue(msg->data);
00452 
00453     }
00454 
00455     void moveInterface(const std_msgs::Float32MultiArray::ConstPtr& msg) {
00456         std::vector<double> pos;
00457         pos.push_back(msg->data[0]);
00458         pos.push_back(msg->data[1]);
00459         pos.push_back(msg->data[2]);
00460         pos.push_back(msg->data[3]);
00461         pos.push_back(msg->data[4]);
00462         pos.push_back(msg->data[5]);
00463 
00464         robot_.setBlock(false);
00465         robot_.movej(pos);
00466     }
00467 
00468         void rosControlLoop() {
00469                 ros::Duration elapsed_time;
00470                 struct timespec last_time, current_time;
00471                 static const double BILLION = 1000000000.0;
00472 
00473         ros::Rate loop_rate(100);
00474 
00475                 clock_gettime(CLOCK_MONOTONIC, &last_time);
00476                 while (ros::ok()) {
00477 
00478                         // Input
00479                         hardware_interface_->read();
00480                         robot_.rt_interface_->robot_state_->setControllerUpdated();
00481 
00482                         // Control
00483                         clock_gettime(CLOCK_MONOTONIC, &current_time);
00484                         elapsed_time = ros::Duration(current_time.tv_sec - last_time.tv_sec + (current_time.tv_nsec - last_time.tv_nsec)/ BILLION);
00485                         controller_manager_->update(ros::Time::now(), elapsed_time);
00486                         last_time = current_time;
00487 
00488                         // Output
00489                         hardware_interface_->write();
00490 
00491             loop_rate.sleep();
00492                 }
00493         }
00494 
00495         void publishRTMsg() {
00496                 ros::Publisher joint_pub = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
00497                 ros::Publisher wrench_pub = nh_.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
00498         //ros::Publisher tool_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("tool_velocity", 1);
00499         static tf::TransformBroadcaster br;
00500                 while (ros::ok()) {
00501                         sensor_msgs::JointState joint_msg;
00502                         joint_msg.name = robot_.getJointNames();
00503                         geometry_msgs::WrenchStamped wrench_msg;
00504             geometry_msgs::PoseStamped tool_pose_msg;
00505 
00506                         std::mutex msg_lock; // The values are locked for reading in the class, so just use a dummy mutex
00507                         std::unique_lock<std::mutex> locker(msg_lock);
00508 
00509                         while (!robot_.rt_interface_->robot_state_->getDataPublished()) {
00510                                 rt_msg_cond_.wait(locker);
00511                         }
00512 
00513                         joint_msg.header.stamp = ros::Time::now();
00514             joint_msg.position = robot_.rt_interface_->robot_state_->getJonitPosition();
00515                         for (unsigned int i = 0; i < joint_msg.position.size(); i++) {
00516                                 joint_msg.position[i] += joint_offsets_[i];
00517                         }
00518             joint_msg.velocity = robot_.rt_interface_->robot_state_->getJonitVelocity();
00519             joint_msg.effort = robot_.rt_interface_->robot_state_->getJointCurrent();
00520                         joint_pub.publish(joint_msg);
00521 
00522             std::vector<double> tcp_force = robot_.rt_interface_->robot_state_->getTcpForce();
00523                         wrench_msg.header.stamp = joint_msg.header.stamp;
00524                         wrench_msg.wrench.force.x = tcp_force[0];
00525                         wrench_msg.wrench.force.y = tcp_force[1];
00526                         wrench_msg.wrench.force.z = tcp_force[2];
00527                         wrench_msg.wrench.torque.x = tcp_force[3];
00528                         wrench_msg.wrench.torque.y = tcp_force[4];
00529                         wrench_msg.wrench.torque.z = tcp_force[5];
00530                         wrench_pub.publish(wrench_msg);
00531 
00532                     // 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
00533             std::vector<double> tool_orientation = robot_.rt_interface_->robot_state_->getToolOrientation();
00534 
00535                     //Create quaternion
00536                     tf::Quaternion quat;
00537             double w = tool_orientation[0];
00538             double x = tool_orientation[1];
00539             double y = tool_orientation[2];
00540             double z = tool_orientation[3];
00541 
00542             double rx = atan2(2.0*(w*x+y*z),1-2.0*(x*x+y*y));
00543             double ry = asin(2.0*(w*y-z*x));
00544             double rz = atan2(2.0*(w*x+x*y),1-2.0*(y*y+z*z));
00545 
00546                     double angle = std::sqrt(std::pow(rx,2) + std::pow(ry,2) + std::pow(rz,2));
00547                     if (angle < 1e-16) {
00548             quat.setValue(0, 0, 0, 1);
00549                     } else {
00550                         quat.setRotation(tf::Vector3(rx/angle, ry/angle, rz/angle), angle);
00551                     }
00552 
00553                     //Create and broadcast transform
00554             std::vector<double> tool_position = robot_.rt_interface_->robot_state_->getToolPosition();
00555                     tf::Transform transform;
00556             transform.setOrigin(tf::Vector3(tool_position[0], tool_position[1], tool_position[2]));
00557                     transform.setRotation(quat);
00558                     br.sendTransform(tf::StampedTransform(transform, joint_msg.header.stamp, base_frame_, tool_frame_));
00559 
00560                     //Publish tool velocity
00561             /*std::vector<double> tcp_speed = robot_.rt_interface_->robot_state_->getTcpSpeedActual();
00562                     geometry_msgs::TwistStamped tool_twist;
00563                     tool_twist.header.frame_id = base_frame_;
00564                     tool_twist.header.stamp = joint_msg.header.stamp;
00565                     tool_twist.twist.linear.x = tcp_speed[0];
00566                     tool_twist.twist.linear.y = tcp_speed[1];
00567                     tool_twist.twist.linear.z = tcp_speed[2];
00568                     tool_twist.twist.angular.x = tcp_speed[3];
00569                     tool_twist.twist.angular.y = tcp_speed[4];
00570                     tool_twist.twist.angular.z = tcp_speed[5];
00571             tool_vel_pub.publish(tool_twist);*/
00572 
00573                     robot_.rt_interface_->robot_state_->setDataPublished();
00574                 }
00575         }
00576 };
00577 
00578 int main(int argc, char **argv) {
00579         bool use_sim_time = false;
00580         std::string host;
00581     int reverse_port = 8899;
00582 
00583         ros::init(argc, argv, "aubo_new_driver");
00584         ros::NodeHandle nh;
00585 
00586         if (ros::param::get("use_sim_time", use_sim_time)) {
00587                 print_warning("use_sim_time is set!!");
00588         }
00589 
00590         if (!(ros::param::get("~robot_ip_address", host))) {
00591                 if (argc > 1) {
00592                         print_warning("Please set the parameter robot_ip_address instead of giving it as a command line argument. This method is DEPRECATED");
00593                         host = argv[1];
00594                 } else {
00595                         print_fatal("Could not get robot ip. Please supply it as command line parameter or on the parameter server as robot_ip");
00596                         exit(1);
00597                 }
00598 
00599         }
00600         if ((ros::param::get("~reverse_port", reverse_port))) {
00601                 if((reverse_port <= 0) or (reverse_port >= 65535)) {
00602             print_warning("Reverse port value is not valid (Use number between 1 and 65534. Using default value of 8899");
00603             reverse_port = 8899;
00604                 }
00605         } else
00606         reverse_port = 8899;
00607 
00608     RosWrapper interface(host, reverse_port);
00609 
00610     ros::AsyncSpinner spinner(6);
00611         spinner.start();
00612 
00613         ros::waitForShutdown();
00614 
00615         interface.halt();
00616 
00617         exit(0);
00618 }


aubo_new_driver
Author(s): liuxin
autogenerated on Wed Sep 6 2017 03:06:44