Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Joy.h>
00003 #include <geometry_msgs/Twist.h>
00004
00005
00006
00007
00008
00009
00010
00011 const double max_speed_x = 0.1;
00012 const double max_speed_y = 0.05;
00013 const double max_rotation = 0.2;
00014 double vel_x, vel_y, vel_rot;
00015
00016 void joyCallback( const sensor_msgs::Joy::ConstPtr& joy_msg )
00017 {
00018 if( joy_msg->axes.size() < 3 )
00019 {
00020 ROS_ERROR( "Too few joystick axes: %d (expected: 3+)", joy_msg->axes.size() );
00021 return;
00022 }
00023
00024 ROS_DEBUG("Received [axis0=%f axis1=%f axis2=%f]", joy_msg->axes[0], joy_msg->axes[1], joy_msg->axes[2] );
00025
00026 if(joy_msg->buttons[4] == 1){
00027 vel_x = joy_msg->axes[1] * max_speed_x;
00028 vel_y = joy_msg->axes[0] * max_speed_y;
00029 } else {
00030 vel_x = 0;
00031 vel_y = 0;
00032 }
00033
00034 if(joy_msg->buttons[5] == 1){
00035 vel_rot = joy_msg->axes[2] * max_rotation;
00036 } else {
00037 vel_rot = 0;
00038 }
00039 }
00040
00041 int main(int argc, char** argv)
00042 {
00043 ros::init(argc, argv, "biped_teleop_joy_node");
00044 ros::NodeHandle n;
00045
00046
00047 ros::Publisher vel_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00048
00049 ros::Subscriber joy_sub = n.subscribe( "joy", 10, &joyCallback );
00050
00051 ros::Rate loop_rate(100);
00052 while( ros::ok() )
00053 {
00054 geometry_msgs::Twist twist_msg;
00055 twist_msg.linear.x = vel_x;
00056 twist_msg.linear.y = vel_y;
00057 twist_msg.angular.z = vel_rot;
00058
00059 if (1 || vel_x != 0 || vel_y != 0 || vel_rot != 0) {
00060 vel_pub.publish(twist_msg);
00061 }
00062
00063 loop_rate.sleep();
00064 ros::spinOnce();
00065 }
00066 }
00067