cob_spacenav_teleop: cob_spacenav_teleop: spacenav

00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: cob_interactive_teleop.cpp 674 2012-04-19 13:59:19Z spanel $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by Robo@FIT group.
00009  *
00010  * Author: Zdenek Materna (imaterna@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 1/12/2012
00013  *
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  *
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  *
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00028 #include "cob_spacenav_teleop/spacenav_teleop.h"
00029 
00030 using namespace cob_spacenav_teleop;
00031 
00032 SpaceNavTeleop::SpaceNavTeleop() {
00033 
00034         enabled_ = true;
00035 
00036         tfl_ = new tf::TransformListener();
00037 
00038         ros::param::param<double>("~max_vel_x",params_.max_vel_x,0.3);
00039         ros::param::param<double>("~max_vel_y",params_.max_vel_y,0.2);
00040         ros::param::param<double>("~max_vel_th",params_.max_vel_th,0.3);
00041         ros::param::param<double>("~scale_linear",params_.scale_linear,0.5);
00042         ros::param::param<double>("~scale_angular",params_.scale_angular,0.25);
00043 
00044         ros::param::param<double>("~spacenav/max_val",params_.sn_max_val,350.0);
00045         ros::param::param<double>("~spacenav/min_val_th",params_.sn_min_val_th,0.05);
00046 
00047         ros::param::param<double>("~spacenav/ignore_th_high",params_.ignore_th_high,0.9);
00048         ros::param::param<double>("~spacenav/ignore_th_low",params_.ignore_th_low,0.1);
00049 
00050         ros::param::param<bool>("~spacenav/instant_stop_enabled",params_.instant_stop_enabled,false);
00051 
00052         ros::param::param<bool>("~unsafe_limiter",params_.unsafe_limiter,false);
00053 
00054         ros::param::param<bool>("~use_rviz_cam",params_.use_rviz_cam,false);
00055         ros::param::param<std::string>("~rviz_cam_link",params_.rviz_cam_link,"/rviz_cam");
00056 
00057         params_.robot_base_link = "/base_link";
00058 
00059         if (params_.sn_min_val_th > 0.5) params_.sn_min_val_th = 0.5;
00060         if (params_.sn_min_val_th < 0.0) params_.sn_min_val_th = 0.0;
00061 
00062         // make sure that all values are positive
00063         if (params_.sn_max_val < 0.0) params_.sn_max_val *= -1.0;
00064         if (params_.max_vel_x < 0.0) params_.max_vel_x *= -1;
00065         if (params_.max_vel_y < 0.0) params_.max_vel_y *= -1;
00066         if (params_.max_vel_th < 0.0) params_.max_vel_th *= -1;
00067 
00068         // and limit maximal values
00069 
00070   btns_.left = false;
00071   btns_.right = false;
00072   btns_.right_last = false;
00073   btns_.right_trigger = false;
00074 
00075         stop_detected_ = false;
00076         //some_dir_limited_ = false;
00077         x_pref_ = false;
00078         y_pref_ = false;
00079         z_pref_ = false;
00080 
00081         pref_time_ = ros::Time(0);
00082 
00083         bp_.header.frame_id = params_.rviz_cam_link;
00084         bp_.header.stamp = ros::Time(0);
00085         bp_.pose.position.x = 0;
00086         bp_.pose.position.y = 0;
00087         bp_.pose.position.z = 0;
00088         bp_.pose.orientation.x = 0.0;
00089         bp_.pose.orientation.y = 0.0;
00090         bp_.pose.orientation.z = 0.0;
00091         bp_.pose.orientation.w = 1.0;
00092 
00093         sn_data_.last_data = ros::Time(0);
00094         sn_data_.last_nonzero_data = ros::Time(0);
00095 
00096         ros::NodeHandle nh("~");
00097 
00098         offset_sub_ = nh.subscribe("/spacenav/offset",3,&SpaceNavTeleop::spacenavOffsetCallback,this);
00099         rot_offset_sub_ = nh.subscribe("/spacenav/rot_offset",3,&SpaceNavTeleop::spacenavRotOffsetCallback,this);
00100         joy_sub_ = nh.subscribe<sensor_msgs::Joy>("/spacenav/joy",3,&SpaceNavTeleop::joyCallback,this);
00101 
00102         twist_publisher_safe_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_safe",3);
00103         twist_publisher_unsafe_ = nh.advertise<geometry_msgs::Twist>("/cmd_vel_unsafe",3);
00104 
00105         publishing_to_unsafe_ = false;
00106         robot_centric_mode_ = false;
00107 
00108         // 50 Hz
00109         timer_ = nh.createTimer(ros::Duration(0.02),&SpaceNavTeleop::timerCallback,this);
00110 
00111         service_en_ = nh.advertiseService("enable", &SpaceNavTeleop::Enable,this);
00112         service_dis_ = nh.advertiseService("disable", &SpaceNavTeleop::Disable,this);
00113 
00114         if (params_.instant_stop_enabled) ROS_INFO("Instant stop feature enabled.");
00115         else ROS_INFO("Instant stop feature disabled.");
00116 
00117         if (params_.unsafe_limiter) ROS_INFO("Unsafe limiter activated.");
00118         else ROS_INFO("Unsafe limiter NOT activated.");
00119 
00120         ROS_INFO("Initiated...");
00121 
00122 
00123 }
00124 
00125 SpaceNavTeleop::~SpaceNavTeleop() {
00126 
00127         delete tfl_;
00128 
00129 
00130 }
00131 
00132 void SpaceNavTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy) {
00133 
00134         if ((int)(joy->buttons.size()) != 2) return;
00135 
00136         btns_.mutex.lock();
00137 
00138         btns_.right_last = btns_.right;
00139 
00140         btns_.left = joy->buttons[0];
00141         btns_.right = joy->buttons[1];
00142 
00143         // activate "triger" only if teleop is enabled
00144         if (enabled_ && (!btns_.right_last) && btns_.right) btns_.right_trigger = true;
00145 
00146         btns_.mutex.unlock();
00147 
00148 }
00149 
00150 bool SpaceNavTeleop::Enable(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00151 
00152         ROS_INFO("Spacenav teleop enabled.");
00153         enabled_ = true;
00154         return true;
00155 
00156 }
00157 
00158 bool SpaceNavTeleop::Disable(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
00159 
00160         ROS_INFO("Spacenav teleop disabled.");
00161         enabled_ = false;
00162         return true;
00163 
00164 }
00165 
00166 void SpaceNavTeleop::normAngle(double& a) {
00167 
00168         if (a > 2*M_PI) {
00169 
00170                 a -= 2*M_PI;
00171                 return;
00172 
00173         }
00174 
00175         if (a < 0) {
00176 
00177                 a = 2*M_PI + a;
00178 
00179         }
00180 
00181         return;
00182 
00183 }
00184 
00185 // Return the rotation in Euler angles
00186 geometry_msgs::Vector3 SpaceNavTeleop::GetAsEuler(geometry_msgs::Quaternion quat)
00187 {
00188   geometry_msgs::Vector3 vec;
00189 
00190   btQuaternion q;
00191 
00192   btScalar roll, pitch, yaw;
00193 
00194   tf::quaternionMsgToTF(quat, q);
00195   btMatrix3x3(q).getRPY(roll,pitch,yaw);
00196 
00197   vec.x = (double)roll;
00198   vec.y = (double)pitch;
00199   vec.z = (double)yaw;
00200 
00201   normAngle(vec.x);
00202   normAngle(vec.y);
00203   normAngle(vec.z);
00204 
00205   return vec;
00206 
00207 }
00208 
00209 
00210 void SpaceNavTeleop::timerCallback(const ros::TimerEvent& ev) {
00211 
00212         ROS_INFO_ONCE("Timer triggered.");
00213 
00214         if (sn_data_.offset_received && sn_data_.rot_offset_received) ROS_INFO_ONCE("COB SpaceNav Teleop is running.");
00215         else return;
00216 
00217         if ( params_.use_rviz_cam && (!tfl_->frameExists(params_.rviz_cam_link)) ) {
00218 
00219                 ROS_ERROR_ONCE("We are going to use RVIZ camera position but it's TF frame (%s) doesn't exist! This message will apper only once.",params_.rviz_cam_link.c_str());
00220                 return;
00221 
00222         }
00223 
00224         if (!enabled_) return;
00225 
00226         if (twist_publisher_safe_.getNumSubscribers() == 0) {
00227 
00228                 ROS_WARN_ONCE("We have no subscriber on main (safe) topic.");
00229 
00230                 return;
00231 
00232         } else {
00233 
00234                 ROS_INFO_ONCE("Someone is subscribed. Starting publishing...");
00235 
00236         }
00237 
00238         geometry_msgs::Twist tw;
00239 
00240         tw.linear.x = 0.0;
00241         tw.linear.y = 0.0;
00242         tw.linear.z = 0.0;
00243 
00244         tw.angular.x = 0.0;
00245         tw.angular.y = 0.0;
00246         tw.angular.z = 0.0;
00247 
00248         geometry_msgs::Vector3 offset;
00249         geometry_msgs::Vector3 rot_offset;
00250 
00251         ros::Time last_data;
00252 
00253         sn_data_.mutex.lock();
00254 
00255         offset = sn_data_.offset;
00256         rot_offset = sn_data_.rot_offset;
00257         last_data = sn_data_.last_data;
00258 
00259         sn_data_.mutex.unlock();
00260 
00261         // instant STOP
00262         if ( ((ros::Time::now() - last_data) > ros::Duration(0.5)) && (last_data != ros::Time(0)) ) {
00263 
00264                 // spacenav node probably crashed ?
00265                 ROS_WARN("Old data");
00266                 twist_publisher_safe_.publish(tw);
00267 
00268                 return;
00269 
00270         }
00271 
00272 
00273         if (!stop_detected_) {
00274 
00275                 if ( (offset.z < -0.85) && params_.instant_stop_enabled) {
00276 
00277                         stop_detected_ = true;
00278                         ROS_WARN("Instant stop!");
00279                         time_of_stop_ = ros::Time::now();
00280 
00281                         twist_publisher_safe_.publish(tw);
00282                         return;
00283 
00284                 }
00285 
00286 
00287         } else {
00288 
00289                 if ((ros::Time::now() - time_of_stop_) > ros::Duration(5.0)) {
00290 
00291                         stop_detected_ = false;
00292                         ROS_INFO("Releasing instant stop.");
00293 
00294                 } else {
00295 
00296                         twist_publisher_safe_.publish(tw);
00297                         return;
00298 
00299                 }
00300 
00301         }
00302 
00303         if ( (ros::Time::now() - sn_data_.last_nonzero_data ) > ros::Duration(1.0)) {
00304 
00305                 // we have nothing to publish
00306                 return;
00307 
00308         }
00309 
00310         // allow different ways of control
00311         if (fabs(rot_offset.y) > fabs(offset.x)) offset.x = rot_offset.y;
00312         if (fabs(rot_offset.x) > fabs(offset.y)) offset.y = -rot_offset.x;
00313 
00314 
00315         // filter out too small values
00316         if (fabs(offset.x) < params_.sn_min_val_th ) offset.x = 0;
00317         if (fabs(offset.y) < params_.sn_min_val_th ) offset.y = 0;
00318         if (fabs(rot_offset.z) < params_.sn_min_val_th ) rot_offset.z = 0;
00319 
00320         bool unsafe = false;
00321         bool mode_trigger = false;
00322 
00323         btns_.mutex.lock();
00324         unsafe = btns_.left;
00325         mode_trigger = btns_.right_trigger;
00326         btns_.right_trigger = false;
00327 
00328         btns_.mutex.unlock();
00329 
00330         if (!params_.use_rviz_cam) {
00331 
00332                 ROS_INFO_ONCE("Started in mode without using RVIZ camera position.");
00333                 robot_centric_mode_ = true;
00334 
00335         } else {
00336 
00337                 if (robot_centric_mode_) {
00338 
00339                         if (mode_trigger) {
00340 
00341                                 ROS_INFO("Switching back to user centric mode.");
00342                                 robot_centric_mode_ = false;
00343 
00344                         }
00345 
00346                 } else {
00347 
00348                         if (mode_trigger) {
00349 
00350                                 robot_centric_mode_ = true;
00351                                 ROS_INFO("Switching to robot centric mode.");
00352 
00353                         }
00354 
00355                 }
00356 
00357         }
00358 
00359         if (!robot_centric_mode_) {
00360 
00361         // transformation of velocities vector is not needed for turning in place
00362         //if (!rot) {
00363 
00364                 ROS_INFO_ONCE("Started in mode with using RVIZ camera position.");
00365 
00366                 // this is necessary only if we don't want to rotate
00367 
00368                 geometry_msgs::PoseStamped bp = bp_;
00369 
00370                 if (!transf(params_.robot_base_link,bp)) return;
00371 
00372                 geometry_msgs::Vector3 rpy = GetAsEuler(bp.pose.orientation);
00373 
00374                 // this is angle between robot base_link and RVIZ camera
00375                 ROS_DEBUG("YAW: %f",(rpy.z/(2*M_PI))*360);
00376 
00377                 Eigen::Vector3f vec((float)offset.x,(float)offset.y,(float)offset.z);
00378 
00379                 Eigen::AngleAxisf tr((float)rpy.z, Eigen::Vector3f::UnitZ());
00380 
00381                 // we are rotating velocities vector around Z axis
00382                 vec = tr * vec;
00383 
00384                 offset.x = (double)vec(0);
00385                 offset.y = (double)vec(1);
00386                 offset.z = (double)vec(2);
00387 
00388         }
00389 
00390 
00391         // will we prefer some direction?
00392         if ( (fabs(offset.x) > params_.ignore_th_high) && (fabs(offset.y) < params_.ignore_th_low) && (fabs(rot_offset.z) < params_.ignore_th_low)) {
00393 
00394                 if (!x_pref_) {
00395 
00396                         x_pref_ = true;
00397                         y_pref_ = false;
00398                         z_pref_ = false;
00399                         pref_time_ = ros::Time::now();
00400                         ROS_INFO("Preferring x dir");
00401 
00402                 }
00403 
00404         } else
00405 
00406         if ( (fabs(offset.y) > params_.ignore_th_high) && (fabs(offset.x) < params_.ignore_th_low) && (fabs(rot_offset.z) < params_.ignore_th_low) ) {
00407 
00408                 if (!y_pref_) {
00409 
00410                         x_pref_ = false;
00411                         y_pref_ = true;
00412                         z_pref_ = false;
00413                         pref_time_ = ros::Time::now();
00414                         ROS_INFO("Preferring y dir");
00415 
00416                 }
00417 
00418         } else
00419 
00420         if ( (fabs(offset.z) > params_.ignore_th_high) && (fabs(offset.x) < params_.ignore_th_low) && (fabs(rot_offset.y) < params_.ignore_th_low) ) {
00421 
00422                 if (!z_pref_) {
00423 
00424                         x_pref_ = false;
00425                         y_pref_ = false;
00426                         z_pref_ = true;
00427                         pref_time_ = ros::Time::now();
00428                         ROS_INFO("Preferring z rot");
00429 
00430                 }
00431 
00432         } else {
00433 
00434 
00435                 if (x_pref_ || y_pref_ || z_pref_) {
00436 
00437                         if ( (ros::Time::now() - pref_time_) > ros::Duration(0.5) ) {
00438 
00439                                 x_pref_ = false;
00440                                 y_pref_ = false;
00441                                 z_pref_ = false;
00442 
00443                                 ROS_INFO("No dir preferred.");
00444 
00445                         }
00446 
00447                 }
00448 
00449         }
00450 
00451         if (x_pref_) {
00452 
00453                 offset.y = 0.0;
00454                 rot_offset.z = 0.0;
00455 
00456         }
00457 
00458 
00459         if (y_pref_) {
00460 
00461                 offset.x = 0.0;
00462                 rot_offset.z = 0.0;
00463 
00464         }
00465 
00466         if (z_pref_) {
00467 
00468                 offset.x = 0.0;
00469                 rot_offset.y = 0.0;
00470 
00471         }
00472 
00473 
00474         // scale to velocities for COB
00475         offset.x *= params_.max_vel_x;
00476         offset.y *= params_.max_vel_y;
00477 
00478         rot_offset.z *= params_.max_vel_th;
00479 
00480 
00481         tw.linear = offset;
00482         tw.angular = rot_offset;
00483 
00484 
00485         if (unsafe) {
00486 
00487                 if (!publishing_to_unsafe_) {
00488 
00489                         publishing_to_unsafe_ = true;
00490                         ROS_WARN("Publishing to unsafe topic!");
00491 
00492                 }
00493 
00494                 if (params_.unsafe_limiter) {
00495 
00496                         tw.angular.z = 0.0;
00497                         tw.linear.x = 0.0;
00498                         tw.linear.y /= 3.0;
00499 
00500                 }
00501 
00502                 twist_publisher_unsafe_.publish(tw);
00503 
00504         } else {
00505 
00506                 if (publishing_to_unsafe_) {
00507 
00508                         ROS_INFO("Publishing to safe topic again.");
00509                         publishing_to_unsafe_ = false;
00510 
00511                 }
00512 
00513                 twist_publisher_safe_.publish(tw);
00514 
00515         }
00516 
00517 
00518         return;
00519 
00520 }
00521 
00522 bool SpaceNavTeleop::transf(std::string target_frame,geometry_msgs::PoseStamped& pose) {
00523 
00524         // transform pose of camera into world
00525         try {
00526 
00527                         if (tfl_->waitForTransform(target_frame, pose.header.frame_id, pose.header.stamp, ros::Duration(2.0))) {
00528 
00529                           tfl_->transformPose(target_frame,pose,pose);
00530 
00531                         } else {
00532 
00533                           ROS_ERROR("Could not get TF transf. from %s into %s.",pose.header.frame_id.c_str(),target_frame.c_str());
00534                           return false;
00535 
00536                         }
00537 
00538         } catch(tf::TransformException& ex){
00539            std::cerr << "Transform error: " << ex.what() << std::endl;
00540 
00541            return false;
00542         }
00543 
00544         return true;
00545 
00546 }
00547 
00548 void SpaceNavTeleop::spacenavOffsetCallback(const geometry_msgs::Vector3ConstPtr& offset) {
00549 
00550         sn_data_.mutex.lock();
00551 
00552         sn_data_.offset = *offset;
00553 
00554         // limit max. value
00555         if (sn_data_.offset.x > params_.sn_max_val) sn_data_.offset.x = params_.sn_max_val;
00556         if (sn_data_.offset.x < -params_.sn_max_val) sn_data_.offset.x = -params_.sn_max_val;
00557 
00558         if (sn_data_.offset.y > params_.sn_max_val) sn_data_.offset.y = params_.sn_max_val;
00559         if (sn_data_.offset.y < -params_.sn_max_val) sn_data_.offset.y = -params_.sn_max_val;
00560 
00561         if (sn_data_.offset.z > params_.sn_max_val) sn_data_.offset.z = params_.sn_max_val;
00562         if (sn_data_.offset.z < -params_.sn_max_val) sn_data_.offset.z = -params_.sn_max_val;
00563 
00564 
00565         // normalize
00566         sn_data_.offset.x /= params_.sn_max_val;
00567         sn_data_.offset.y /= params_.sn_max_val;
00568         sn_data_.offset.z /= params_.sn_max_val;
00569 
00570         sn_data_.last_data = ros::Time::now();
00571 
00572         if (sn_data_.offset.x != 0.0 || sn_data_.offset.y != 0.0) sn_data_.last_nonzero_data = ros::Time::now();
00573 
00574         sn_data_.mutex.unlock();
00575 
00576         if (!sn_data_.offset_received) {
00577 
00578                 sn_data_.offset_received = true;
00579                 ROS_INFO("Offset received.");
00580 
00581         }
00582 
00583 }
00584 
00585 
00586 void SpaceNavTeleop::spacenavRotOffsetCallback(const geometry_msgs::Vector3ConstPtr& rot_offset) {
00587 
00588         sn_data_.mutex.lock();
00589 
00590         sn_data_.rot_offset = *rot_offset;
00591 
00592         if (sn_data_.rot_offset.x > params_.sn_max_val) sn_data_.rot_offset.x = params_.sn_max_val;
00593         if (sn_data_.rot_offset.x < -params_.sn_max_val) sn_data_.rot_offset.x = -params_.sn_max_val;
00594 
00595 
00596         if (sn_data_.rot_offset.y > params_.sn_max_val) sn_data_.rot_offset.y = params_.sn_max_val;
00597         if (sn_data_.rot_offset.y < -params_.sn_max_val) sn_data_.rot_offset.y = -params_.sn_max_val;
00598 
00599         if (sn_data_.rot_offset.z > params_.sn_max_val) sn_data_.rot_offset.z = params_.sn_max_val;
00600         if (sn_data_.rot_offset.z < -params_.sn_max_val) sn_data_.rot_offset.z = -params_.sn_max_val;
00601 
00602         sn_data_.rot_offset.x /= params_.sn_max_val;
00603         sn_data_.rot_offset.y /= params_.sn_max_val;
00604         sn_data_.rot_offset.z /= params_.sn_max_val;
00605 
00606         if (sn_data_.rot_offset.z != 0.0) sn_data_.last_nonzero_data = ros::Time::now();
00607 
00608         sn_data_.mutex.unlock();
00609 
00610         if (!sn_data_.rot_offset_received) {
00611 
00612                 sn_data_.rot_offset_received = true;
00613                 ROS_INFO("Rot. offset received.");
00614 
00615         }
00616 
00617 }
00618 
00619 
00620 int main(int argc, char** argv)
00621 {
00622   ros::init(argc, argv, "cob_spacenav_teleop");
00623 
00624   ROS_INFO("Starting COB SpaceNav Teleop...");
00625   SpaceNavTeleop sp;
00626 
00627   /*ros::AsyncSpinner spinner(3);
00628   spinner.start();
00629   ros::waitForShutdown();*/
00630 
00631   ros::spin();
00632 
00633 }
spacenav_teleop.cpp
