remote_joy.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Joy.h>
00003 #include <std_srvs/Trigger.h>
00004 #include <nav2d_operator/cmd.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 mStopClient;
00035         ros::ServiceClient mPauseClient;
00036         ros::ServiceClient mExploreClient;
00037         ros::ServiceClient mGetMapClient;
00038         
00039         int mAxisVelocity;
00040         int mAxisDirection;
00041         int mButtonDriveMode;
00042         int mButtonPauseNavigator;
00043         int mButtonStartExploration;
00044         int mButtonGetMap;
00045         int mButtonStop;
00046         
00047         bool mButtonPressed;
00048 };
00049 
00050 Teleoperator::Teleoperator()
00051 {
00052         // Button and Axis configuration
00053         mAxisVelocity = 4;
00054         mAxisDirection = 0;
00055 
00056         mButtonDriveMode = 5;
00057         mButtonPauseNavigator = 6;
00058         mButtonStartExploration = 0;
00059         mButtonGetMap = 3;
00060         mButtonStop = 1;
00061         
00062         mCommandPublisher = mNode.advertise<nav2d_operator::cmd>("cmd", 1);
00063         mJoySubscriber = mNode.subscribe<sensor_msgs::Joy>("joy", 10, &Teleoperator::joyCB, this);
00064         mStopClient = mNode.serviceClient<std_srvs::Trigger>(NAV_STOP_SERVICE);
00065         mPauseClient = mNode.serviceClient<std_srvs::Trigger>(NAV_PAUSE_SERVICE);
00066         mExploreClient = mNode.serviceClient<std_srvs::Trigger>(NAV_EXPLORE_SERVICE);
00067         mGetMapClient = mNode.serviceClient<std_srvs::Trigger>(NAV_GETMAP_SERVICE);
00068         
00069         mButtonPressed = false;
00070 }
00071 
00072 void Teleoperator::joyCB(const sensor_msgs::Joy::ConstPtr& msg)
00073 {
00074         // Ignore Button-Release events
00075         if(mButtonPressed)
00076         {
00077                 mButtonPressed = false;
00078         }else
00079         {       
00080                 nav2d_operator::cmd cmd;
00081                 cmd.Turn = msg->axes[mAxisDirection] * -1.0;
00082                 cmd.Velocity = msg->axes[mAxisVelocity];
00083                 cmd.Mode = 0;
00084                 if(msg->buttons[mButtonDriveMode]) cmd.Mode = 1;
00085                 mCommandPublisher.publish(cmd);
00086         }
00087 
00088         if(msg->buttons[mButtonStop])
00089         {
00090                 std_srvs::Trigger srv;
00091                 if(!mStopClient.call(srv))
00092                 {
00093                         ROS_ERROR("Failed to send STOP_COMMAND to Navigator.");
00094                 }
00095                 return;
00096         }
00097 
00098         if(msg->buttons[mButtonPauseNavigator])
00099         {
00100                 std_srvs::Trigger srv;
00101                 if(!mPauseClient.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                 std_srvs::Trigger srv;
00111                 if(!mGetMapClient.call(srv))
00112                 {
00113                         ROS_ERROR("Failed to send GETMAP_COMMAND to GetMap-Client.");
00114                 }
00115                 mButtonPressed = true;
00116                 return;
00117         }
00118 
00119         if(msg->buttons[mButtonStartExploration])
00120         {
00121                 std_srvs::Trigger srv;
00122                 if(!mExploreClient.call(srv))
00123                 {
00124                         ROS_ERROR("Failed to send EXPLORE_COMMAND to Explore-Client.");
00125                 }
00126                 mButtonPressed = true;
00127                 return;
00128         }
00129 }
00130 
00131 int main(int argc, char** argv)
00132 {
00133         ros::init(argc, argv, "teleoperator");
00134         Teleoperator tele_op;
00135         
00136         ros::spin();
00137 }


nav2d_remote
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 03:53:50