remote_joy.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Joy.h>
00003 #include <nav2d_operator/cmd.h>
00004 #include <nav2d_navigator/SendCommand.h>
00005 #include <nav2d_navigator/commands.h>
00006 
00007 /******************************************************
00008 Buttons:
00009  0: A
00010  1: B
00011  2: X
00012  3: Y
00013  4: LB
00014  5: RB
00015  6: BACK
00016  7: START
00017  8: Logitech*
00018  9: Left Stick
00019 10: Right Stick
00020 
00021  ******************************************************/
00022 
00023 class Teleoperator
00024 {
00025 public:
00026         Teleoperator();
00027         
00028 private:
00029         void joyCB(const sensor_msgs::Joy::ConstPtr& msg);
00030 
00031         ros::NodeHandle mNode;
00032         ros::Publisher mCommandPublisher;
00033         ros::Subscriber mJoySubscriber;
00034         ros::ServiceClient mNavigatorClient;
00035         ros::ServiceClient mExploreClient;
00036         ros::ServiceClient mGetMapClient;
00037         
00038         int mAxisVelocity;
00039         int mAxisDirection;
00040         int mButtonDriveMode;
00041         int mButtonPauseNavigator;
00042         int mButtonStartExploration;
00043         int mButtonGetMap;
00044         int mButtonStop;
00045         
00046         bool mButtonPressed;
00047 };
00048 
00049 Teleoperator::Teleoperator()
00050 {
00051         // Button and Axis configuration
00052         mAxisVelocity = 4;
00053         mAxisDirection = 0;
00054 
00055         mButtonDriveMode = 5;
00056         mButtonPauseNavigator = 6;
00057         mButtonStartExploration = 0;
00058         mButtonGetMap = 3;
00059         mButtonStop = 1;
00060         
00061         mCommandPublisher = mNode.advertise<nav2d_operator::cmd>("cmd", 1);
00062         mJoySubscriber = mNode.subscribe<sensor_msgs::Joy>("joy", 10, &Teleoperator::joyCB, this);
00063         mNavigatorClient = mNode.serviceClient<nav2d_navigator::SendCommand>(NAV_COMMAND_SERVICE);
00064         mExploreClient = mNode.serviceClient<nav2d_navigator::SendCommand>(NAV_EXPLORE_SERVICE);
00065         mGetMapClient = mNode.serviceClient<nav2d_navigator::SendCommand>(NAV_GETMAP_SERVICE);
00066         
00067         mButtonPressed = false;
00068 }
00069 
00070 void Teleoperator::joyCB(const sensor_msgs::Joy::ConstPtr& msg)
00071 {
00072         // Ignore Button-Release events
00073         if(mButtonPressed)
00074         {
00075                 mButtonPressed = false;
00076         }else
00077         {       
00078                 nav2d_operator::cmd cmd;
00079                 cmd.Turn = msg->axes[mAxisDirection] * -1.0;
00080                 cmd.Velocity = msg->axes[mAxisVelocity];
00081                 cmd.Mode = 0;
00082                 if(msg->buttons[mButtonDriveMode]) cmd.Mode = 1;
00083                 mCommandPublisher.publish(cmd);
00084         }
00085 
00086         if(msg->buttons[mButtonStop])
00087         {
00088                 nav2d_navigator::SendCommand srv;
00089                 srv.request.command = NAV_COM_STOP;
00090                 if(!mNavigatorClient.call(srv))
00091                 {
00092                         ROS_ERROR("Failed to send STOP_COMMAND to Navigator.");
00093                 }
00094                 return;
00095         }
00096 
00097         if(msg->buttons[mButtonPauseNavigator])
00098         {
00099                 nav2d_navigator::SendCommand srv;
00100                 srv.request.command = NAV_COM_PAUSE;
00101                 if(!mNavigatorClient.call(srv))
00102                 {
00103                         ROS_ERROR("Failed to send PAUSE_COMMAND to Navigator.");
00104                 }
00105                 return;
00106         }
00107 
00108         if(msg->buttons[mButtonGetMap])
00109         {
00110                 nav2d_navigator::SendCommand srv;
00111                 srv.request.command = NAV_COM_GETMAP;
00112                 if(!mGetMapClient.call(srv))
00113                 {
00114                         ROS_ERROR("Failed to send GETMAP_COMMAND to GetMap-Client.");
00115                 }
00116                 mButtonPressed = true;
00117                 return;
00118         }
00119 
00120         if(msg->buttons[mButtonStartExploration])
00121         {
00122                 nav2d_navigator::SendCommand srv;
00123                 srv.request.command = NAV_COM_EXPLORE;
00124                 if(!mExploreClient.call(srv))
00125                 {
00126                         ROS_ERROR("Failed to send EXPLORE_COMMAND to Explore-Client.");
00127                 }
00128                 mButtonPressed = true;
00129                 return;
00130         }
00131 }
00132 
00133 int main(int argc, char** argv)
00134 {
00135         ros::init(argc, argv, "teleoperator");
00136         Teleoperator tele_op;
00137         
00138         ros::spin();
00139 }


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