1  /*
   2  * Copyright (c) 2013, Anas Alhashimi LTU.
   3  * All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions are met:
   7  *
   8  *     * Redistributions of source code must retain the above copyright
   9  *       notice, this list of conditions and the following disclaimer.
  10  *     * Redistributions in binary form must reproduce the above copyright
  11  *       notice, this list of conditions and the following disclaimer in the
  12  *       documentation and/or other materials provided with the distribution.
  13  *     * Neither the name of the Willow Garage, Inc. nor the names of its
  14  *       contributors may be used to endorse or promote products derived from
  15  *       this software without specific prior written permission.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  27  * POSSIBILITY OF SUCH DAMAGE.
  28  * Anas W. Alhahsimi
  29  */
  30 
  31 #include <ros/ros.h>
  32 #include "geometry_msgs/Twist.h"
  33 #include "ROSARIA/BumperState.h"
  34 #include "sensor_msgs/Imu.h"
  35 #include <signal.h>
  36 #include <termios.h>
  37 
  38 
  39 using geometry_msgs::Twist;
  40 using namespace std;
  41 
  42 ros::Publisher chatter_pub;
  43 ros::Time t1;
  44 Twist vel;
  45 int kfd = 0;
  46 struct termios cooked, raw;
  47 unsigned int temp=0;
  48 float x;
  49 float y;
  50 float z;
  51 
  52 
  53 void quit(int sig)
  54    {
  55      tcsetattr(kfd, TCSANOW, &cooked);
  56      ros::shutdown();
  57      exit(0);
  58    }
  59 
  60 
  61 
  62 
  63 void anCallback(const sensor_msgs::Imu::ConstPtr& ansmsg)
  64 {
  65  x=ansmsg->angular_velocity.x;
  66  y=ansmsg->angular_velocity.y;
  67  z=ansmsg->angular_velocity.z;
  68  
  69 
  70  }
  71 
  72 
  73 
  74 
  75 
  76 int main(int argc, char** argv)
  77 { 
  78   int ther=5;
  79 
  80   ros::init(argc, argv, "android_teleop");
  81   ros::NodeHandle n;
  82    chatter_pub = n.advertise<Twist>("/cmd_vel", 1);  
  83    signal(SIGINT,quit);
  84         ros::Rate r(5);
  85    char c;
  86    bool dirty=false;   
  87    t1=ros::Time::now();
  88 
  89    tcgetattr(kfd, &cooked);
  90    memcpy(&raw, &cooked, sizeof(struct termios));
  91    raw.c_lflag &=~ (ICANON | ECHO);
  92    raw.c_cc[VEOL] = 1;
  93    raw.c_cc[VEOF] = 2;
  94    tcsetattr(kfd, TCSANOW, &raw);
  95 
  96 //subscribe to android imu sensor msgs
  97         ros::Subscriber imu_pub = n.subscribe<sensor_msgs::Imu>("/android/imu", 1, anCallback);
  98  
  99 
 100  while (ros::ok())
 101   {     
 102 
 103 
 104 
 105 if(x > ther)
 106            {
 107       vel.linear.x = 0.2;
 108       vel.linear.y=0;
 109       vel.linear.z=0;
 110       vel.angular.x = 0;
 111       vel.angular.y = 0;
 112       vel.angular.z = 0;
 113         ROS_INFO("forward");
 114 
 115 }
 116 if(x < -ther)
 117            {
 118         vel.linear.x = -0.2;
 119         vel.linear.y=0;
 120         vel.linear.z=0;
 121         vel.angular.x = 0;
 122         vel.angular.y = 0;
 123         vel.angular.z = 0;
 124         ROS_INFO("Backward");
 125 }
 126      
 127 if(z > ther)
 128            {
 129       vel.linear.x = 0;
 130       vel.linear.y=0;
 131       vel.linear.z=0;
 132       vel.angular.x = 0;
 133       vel.angular.y = 0;
 134       vel.angular.z = 0.5;
 135         ROS_INFO("Turnleft");
 136 }
 137 if(z < -ther)
 138            {
 139         vel.linear.x = 0;
 140         vel.linear.y=0;
 141         vel.linear.z=0;
 142         vel.angular.x = 0;
 143         vel.angular.y = 0;
 144         vel.angular.z = -0.5;
 145         ROS_INFO("Turnright");
 146 }
 147 if(y < -ther)
 148            {
 149         vel.linear.x = 0;
 150         vel.linear.y=0;
 151         vel.linear.z=0;
 152         vel.angular.x = 0;
 153         vel.angular.y = 0;
 154         vel.angular.z = 0;
 155         ROS_INFO("stop");
 156 }
 157 if(y > ther)
 158            {
 159         vel.linear.x = 0;
 160         vel.linear.y=0;
 161         vel.linear.z=0;
 162         vel.angular.x = 0;
 163         vel.angular.y = 0;
 164         vel.angular.z = 0;
 165         ROS_INFO("stop");
 166 }
 167  
 168          
 169         
 170 
 171        chatter_pub.publish(vel);
 172              
 173 ros::Duration(0.1).sleep(); // sleep for one tenth of a second
 174        
 175    
 176     ros::spinOnce();
 177    
 178   }//while
 179 return(0);
 180 }