kurt_teleop_ps3joy.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <geometry_msgs/Twist.h>
00003 #include <sensor_msgs/Joy.h>
00004 
00005 double max_vel_x, max_rotational_vel;
00006 ros::Publisher vel_pub;
00007 double speed_multiplier;
00008 
00009 void ps3joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
00010 {
00011   geometry_msgs::Twist vel;
00012   if (joy->buttons[8] == 1)   // check for full-speed button
00013   {
00014     speed_multiplier = 1.0;
00015   }
00016   else if (true)  // check if right analog stick was used to scale speed
00017   {
00018     speed_multiplier = 0.5 + (0.5 * joy->axes[3]);  // stick full front -> speed_multiplier = 1.0 , full back -> 0.0
00019   }
00020   else  // else use half max speed
00021   {
00022     speed_multiplier = 0.5;
00023   }
00024 
00025   // check if cross is used for steering
00026   if (joy->buttons[4] || joy->buttons[5] || 
00027       joy->buttons[6] || joy->buttons[7])
00028   {
00029     // note that every button of the cross (axes 4-7) generates 
00030     // an output in [-1.0, 0.0]
00031     vel.linear.x = max_vel_x * (joy->axes[4] * -1.0 + joy->axes[6]) * speed_multiplier;
00032     vel.angular.z = max_rotational_vel * (joy->axes[7] * -1.0 + joy->axes[5]) * speed_multiplier;
00033   }
00034   else  // use left analog stick
00035   {
00036     vel.linear.x = max_vel_x * joy->axes[1] * speed_multiplier;
00037     vel.angular.z = max_rotational_vel * joy->axes[0] * speed_multiplier;
00038   }
00039   vel_pub.publish(vel);
00040 }
00041 
00042 int main(int argc, char** argv)
00043 {
00044   ros::init(argc, argv, "kurt_teleop_ps3joy");
00045 
00046   ros::NodeHandle nh;
00047   ros::NodeHandle nh_ns("~");
00048 
00049   nh_ns.param("max_vel_x", max_vel_x, 1.5);
00050   nh_ns.param("max_rotational_vel", max_rotational_vel, 3.0);
00051 
00052   vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00053   ros::Subscriber ps3joy_sub = nh.subscribe("joy", 10, ps3joyCallback);
00054 
00055   ros::spin();
00056 }
00057 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


kurt_teleop
Author(s): Jochen Sprickerhof
autogenerated on Tue May 28 2013 15:11:57