teleop_gripper.cpp
Go to the documentation of this file.
00001 /*
00002  * teleop_pr2
00003  * Copyright (c) 2008, Willow Garage, Inc.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the <ORGANIZATION> nor the names of its
00015  *       contributors may be used to endorse or promote products derived from
00016  *       this software without specific prior written permission.
00017  *
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <ros/ros.h>
00032 #include <pr2_controllers_msgs/Pr2GripperCommand.h>
00033 #include <sensor_msgs/Joy.h>
00034 
00035 class TeleopGripper
00036 {
00037 public:
00038   TeleopGripper()
00039   {
00040     ros::NodeHandle private_nh_("~");
00041 
00042     private_nh_.param("open_position",   open_cmd_.position,     0.08);
00043     private_nh_.param("open_max_effort", open_cmd_.max_effort,  -1.0);
00044 
00045     private_nh_.param("close_position",   close_cmd_.position,  -100.00);
00046     private_nh_.param("close_max_effort", close_cmd_.max_effort,   -1.0);
00047 
00048     private_nh_.param("open_button",   open_button_,   1);
00049     private_nh_.param("close_button",  close_button_,  2);
00050 
00051     pub_ = nh_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("command", 1, false);
00052     sub_ = nh_.subscribe("joy", 1, &TeleopGripper::joyCallback, this);
00053 
00054     ROS_DEBUG("teleop_gripper started");
00055   }
00056 
00057   void joyCallback(const sensor_msgs::JoyConstPtr& joy)
00058   {
00059     ROS_DEBUG("Got a joy msg");
00060     if ((int) joy->buttons.size() <= open_button_ ||
00061         (int) joy->buttons.size() <= close_button_ ||
00062         open_button_ < 0 ||
00063         close_button_ < 0)
00064     {
00065       ROS_ERROR("Array lookup error: Joystick message has %u elems. Open Index [%u]. Close Index [%u]", (unsigned int)joy->buttons.size(), open_button_, close_button_);
00066       return;
00067     }
00068 
00069     ROS_DEBUG("open: Buttons[%u] = %u", open_button_, joy->buttons[open_button_]);
00070 
00071     if (joy->buttons[open_button_] == 1)
00072     {
00073       pub_.publish(open_cmd_);
00074       ROS_DEBUG("Opening");
00075     }
00076     else if (joy->buttons[close_button_] == 1)
00077     {
00078       pub_.publish(close_cmd_);
00079       ROS_DEBUG("Closing");
00080     }
00081   }
00082 
00083 private:
00084   ros::NodeHandle nh_;
00085   ros::Publisher pub_;
00086   ros::Subscriber sub_;
00087 
00088   int close_button_;
00089   int open_button_;
00090 
00091 
00092   pr2_controllers_msgs::Pr2GripperCommand open_cmd_;
00093   pr2_controllers_msgs::Pr2GripperCommand close_cmd_;
00094 };
00095 
00096 int main(int argc, char** argv)
00097 {
00098   ros::init(argc, argv, "teleop_gripper");
00099 
00100   TeleopGripper teleop_gripper;
00101 
00102   ros::spin();
00103 
00104 }


pr2_teleop
Author(s):
autogenerated on Thu Jun 6 2019 21:08:08