uos_rotunit_teleop: uos_rotunit_teleop

Go to the documentation of this file.
00001 /*
00002  *
00003  * Copyright (C) 2015 University of Osnabrück, Germany
00004  * 
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * uos_rotunit_teleop_ps3joy.cpp
00020  *
00021  *  Created on: 05.03.2015
00022  *      Author: Sebastian Pütz <spuetz@uos.de>
00023  */
00024 
00025 #include "uos_rotunit_teleop_ps3joy.h"
00026 
00027 RotunitTeleopPS3Joy::RotunitTeleopPS3Joy(ros::NodeHandle &nh)
00028   : nh_(nh),
00029     ac("rotunit_snapshotter", true),
00030     vel_(0)
00031 {
00032   ROS_INFO("Waiting for the snapshotter action server to start.");
00033   ac.waitForServer(); 
00034   ROS_INFO("Connected to server.");
00035 
00036   nh_.param("acc", acc_, 0.01);           // acceleration
00037   nh_.param("scan_vel", scan_vel_, 0.6);  // velocity while scanning
00038   vel_pub_ = nh_.advertise<geometry_msgs::Twist>("rot_vel", 1);
00039   joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 15,  &RotunitTeleopPS3Joy::PS3Callback, this);
00040   rot_vel_client_ = nh_.serviceClient<uos_rotunit_driver::RotVelSrv>("rotunit_velocity");
00041 }
00042 
00043 void RotunitTeleopPS3Joy::PS3Callback(const sensor_msgs::Joy::ConstPtr &joy){
00044 
00045   uos_rotunit_driver::RotVelSrv rot_vel_srv;
00046   if(joy->buttons[PS3_BUTTON_REAR_RIGHT_2])
00047     vel_ += acc_;
00048   if(joy->buttons[PS3_BUTTON_REAR_RIGHT_1])
00049     vel_ -= acc_;
00050   
00051   if(vel_ > 1.3) vel_ = 1.3;
00052   if(vel_ < -1.3) vel_ = -1.3;
00053 
00054   rot_vel_srv.request.twist.angular.z = vel_;
00055   rot_vel_client_.call(rot_vel_srv);
00056   
00057   uos_rotunit_snapshotter::RotunitSnapshotGoal goal;
00058   if(joy->buttons[PS3_BUTTON_ACTION_TRIANGLE]){
00059     rot_vel_srv.request.twist.angular.z = scan_vel_;
00060     rot_vel_client_.call(rot_vel_srv);
00061     ROS_INFO("start snapshot in 3 seconds...");
00062     ros::Duration(3.0).sleep();
00063     ROS_INFO("... start scanning.");
00064     goal.angle = 2 * M_PI;
00065     // send a goal to the action
00066     ac.sendGoal(goal);
00067     bool finished_before_timeout =
00068       ac.waitForResult(ros::Duration(30.0));
00069 
00070     if (finished_before_timeout){
00071       actionlib::SimpleClientGoalState state = ac.getState();
00072       ROS_INFO("Action finished: %s",state.toString().c_str());
00073     }
00074     else{
00075       ROS_INFO("Action did not finish before the timeout.");
00076     }
00077     rot_vel_srv.request.twist.angular.z = vel_;
00078     rot_vel_client_.call(rot_vel_srv);
00079   }
00080 }
00081 
00082 int main(int args, char**argv){
00083   ros::init(args, argv, "uos_rotunit_snapshotter_ps3joy");
00084   ros::NodeHandle nh;
00085   RotunitTeleopPS3Joy teleop(nh);
00086   ros::spin();
00087   return EXIT_SUCCESS;
00088 }