Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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 uos_rotunit_snapshotter action server to start.");
00033 ac.waitForServer();
00034 ROS_INFO("Connected to uos_rotunit_snapshotter server.");
00035
00036 nh_.param("acc", acc_, 0.01);
00037 nh_.param("scan_vel", scan_vel_, 0.6);
00038 nh_.param("max_vel", max_vel_, 1.3);
00039 nh_.param("timeout", timeout_, 30.0);
00040 vel_pub_ = nh_.advertise<geometry_msgs::Twist>("rot_vel", 1);
00041 joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 15, &RotunitTeleopPS3Joy::PS3Callback, this);
00042 rot_vel_client_ = nh_.serviceClient<uos_rotunit_driver::RotVelSrv>("rotunit_velocity");
00043 }
00044
00045 void RotunitTeleopPS3Joy::PS3Callback(const sensor_msgs::Joy::ConstPtr &joy){
00046
00047 uos_rotunit_driver::RotVelSrv rot_vel_srv;
00048
00049
00050
00051 if(joy->buttons[PS3_BUTTON_REAR_RIGHT_2])
00052 vel_ += acc_;
00053 if(joy->buttons[PS3_BUTTON_REAR_RIGHT_1])
00054 vel_ -= acc_;
00055
00056
00057 if(vel_ > max_vel_)
00058 vel_ = max_vel_;
00059 if(vel_ < -max_vel_)
00060 vel_ = -max_vel_;
00061
00062 rot_vel_srv.request.twist.angular.z = vel_;
00063 rot_vel_client_.call(rot_vel_srv);
00064
00065
00066 if(joy->buttons[PS3_BUTTON_ACTION_TRIANGLE]){
00067 sound_client_.say("Starting scan");
00068
00069
00070 rot_vel_srv.request.twist.angular.z = scan_vel_;
00071 rot_vel_client_.call(rot_vel_srv);
00072 ROS_INFO("start snapshot in 3 seconds...");
00073 ros::Duration(3.0).sleep();
00074
00075
00076 ROS_INFO("start snapshot...");
00077 uos_rotunit_snapshotter::RotunitSnapshotGoal goal;
00078 goal.angle = 2 * M_PI;
00079 ac.sendGoal(goal);
00080 bool finished_before_timeout =
00081 ac.waitForResult(ros::Duration(timeout_));
00082
00083 if (finished_before_timeout){
00084 actionlib::SimpleClientGoalState state = ac.getState();
00085 ROS_INFO("Snapshot action finished with the state: %s",state.toString().c_str());
00086 sound_client_.say("Finished scan");
00087 }
00088 else{
00089 ROS_INFO("Snapshot action did not finish before the timeout of %f seconds.", timeout_);
00090 sound_client_.say("Scan didn't finish in time");
00091 }
00092
00093
00094 rot_vel_srv.request.twist.angular.z = vel_;
00095 rot_vel_client_.call(rot_vel_srv);
00096 }
00097 }
00098
00099 int main(int args, char**argv){
00100 ros::init(args, argv, "uos_rotunit_teleop_ps3joy");
00101 ros::NodeHandle nh;
00102 RotunitTeleopPS3Joy teleop(nh);
00103 ros::spin();
00104 return EXIT_SUCCESS;
00105 }