00001 #include <ros/ros.h>
00002 #include <geometry_msgs/Twist.h>
00003 #include <std_msgs/String.h>
00004 #include <sensor_msgs/Joy.h>
00005
00006 double max_vel_x, max_rotational_vel;
00007 ros::Publisher vel_pub;
00008 ros::Publisher req_pub;
00009 std_msgs::String request;
00010
00011 bool requesting;
00012
00013 void cyborgevoCallback(const sensor_msgs::Joy::ConstPtr& joy)
00014 {
00015 geometry_msgs::Twist vel;
00016 vel.linear.x = max_vel_x * (joy->axes[2] + 1) / 2 * joy->axes[1];
00017 vel.angular.z = max_rotational_vel * (joy->axes[2] + 1) / 2 * joy->axes[0];
00018 vel_pub.publish(vel);
00019
00020 if (joy->buttons[0] != 0) {
00021 if(!requesting) req_pub.publish(request);
00022 requesting = true;
00023 }
00024 else requesting = false;
00025 }
00026
00027 int main(int argc, char** argv)
00028 {
00029 ros::init(argc, argv, "kurt_teleop_cyborgevo");
00030
00031 ros::NodeHandle nh;
00032 ros::NodeHandle nh_ns("~");
00033
00034 nh_ns.param("max_vel_x", max_vel_x, 1.5);
00035 nh_ns.param("max_rotational_vel", max_rotational_vel, 3.0);
00036
00037 request.data = "scanRequest";
00038 requesting = false;
00039
00040 vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00041 req_pub = nh.advertise<std_msgs::String>("request", 1);
00042 ros::Subscriber cyborgevo_sub = nh.subscribe("joy", 10, cyborgevoCallback);
00043
00044 ros::spin();
00045 }
00046
