sim_joy.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Joy.h>
00003 #include <geometry_msgs/TwistStamped.h>
00004 
00005 #include <string.h>
00006 #include <vector>
00007 
00008 #define COMMAND_TIMEOUT_SEC 0.5
00009 
00010 const int PUBLISH_FREQ = 20;
00011 
00012 class SimOperator
00013 {
00014 public:
00015         SimOperator(unsigned int numOfRobots);
00016 
00017         void receiveJoyMessage(const sensor_msgs::Joy::ConstPtr& msg);
00018         void sendControl();
00019 
00020 private:
00021         unsigned int mNumberOfRobots;
00022         double mMaxLinearVelocity;
00023         double mMaxAngularVelocity;
00024 
00025         ros::Subscriber mJoySubscriber;
00026         std::vector<ros::Publisher> mCommandPublisher;
00027         std::vector<geometry_msgs::Twist> mCommand;
00028 };
00029 
00030 SimOperator::SimOperator(unsigned int numOfRobots)
00031 {
00032         mNumberOfRobots = numOfRobots;
00033         mMaxLinearVelocity = 1.0;
00034         mMaxAngularVelocity = 1.0;
00035 
00036         ros::NodeHandle node;   
00037 
00038         // Subscribe to joystick
00039         mJoySubscriber = node.subscribe("joy", 10, &SimOperator::receiveJoyMessage, this);
00040 
00041         // Publish commands for every robot in the simulation
00042         char topic[50];
00043         for(unsigned int i = 0; i < mNumberOfRobots; i++)
00044         {
00045                 if(mNumberOfRobots > 1)
00046                         sprintf(topic, "/robot_%d/cmd_vel", i);
00047                 else
00048                         sprintf(topic, "/cmd_vel", i);
00049                 mCommandPublisher.push_back(node.advertise<geometry_msgs::Twist>(topic, 1));
00050 
00051                 geometry_msgs::Twist twist;
00052                 mCommand.push_back(twist);
00053         }
00054 }
00055 
00056 void SimOperator::receiveJoyMessage(const sensor_msgs::Joy::ConstPtr& msg)
00057 {
00058         for(unsigned int i = 0; i < mNumberOfRobots; i++)
00059         {
00060                 if(msg->buttons[i])
00061                 {
00062                         mCommand[i].linear.x = msg->axes[1] * mMaxLinearVelocity;
00063                         mCommand[i].angular.z = msg->axes[0] * mMaxAngularVelocity;
00064                 }else
00065                 {
00066                         mCommand[i].linear.x = 0;
00067                         mCommand[i].angular.z = 0;
00068                 }
00069         }
00070 }
00071 
00072 void SimOperator::sendControl()
00073 {
00074         for(unsigned int i = 0; i < mNumberOfRobots; i++)
00075         {
00076                 mCommandPublisher[i].publish(mCommand[i]);
00077         }
00078 }
00079 
00080 int main(int argc, char** argv)
00081 {
00082         // init the ROS node
00083         ros::init(argc,argv,"SimController");
00084         ros::NodeHandle node;
00085 
00086         unsigned int num = 1;
00087         if(argc > 1)
00088         {
00089                 num = (unsigned int)atoi(argv[1]);
00090                 if(num > 6) num = 6;
00091         }
00092         
00093         ROS_INFO("Using Joystick to operate %d robots in simulation!", num);
00094         SimOperator simOp(num);
00095 
00096         ros::Rate pub_rate(PUBLISH_FREQ);
00097         while (node.ok())
00098         {
00099                 ros::spinOnce();
00100                 simOp.sendControl();
00101                 pub_rate.sleep();
00102         }
00103 
00104         return 0;
00105 }
00106 


nav2d_remote
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:19