teleop_gripper.cpp
Go to the documentation of this file.
1 /*
2  * teleop_pr2
3  * Copyright (c) 2008, Willow Garage, Inc.
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  * * Neither the name of the <ORGANIZATION> nor the names of its
15  * contributors may be used to endorse or promote products derived from
16  * this software without specific prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
22  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28  * POSSIBILITY OF SUCH DAMAGE.
29  */
30 
31 #include <ros/ros.h>
32 #include <pr2_controllers_msgs/Pr2GripperCommand.h>
33 #include <sensor_msgs/Joy.h>
34 
36 {
37 public:
39  {
40  ros::NodeHandle private_nh_("~");
41 
42  private_nh_.param("open_position", open_cmd_.position, 0.08);
43  private_nh_.param("open_max_effort", open_cmd_.max_effort, -1.0);
44 
45  private_nh_.param("close_position", close_cmd_.position, -100.00);
46  private_nh_.param("close_max_effort", close_cmd_.max_effort, -1.0);
47 
48  private_nh_.param("open_button", open_button_, 1);
49  private_nh_.param("close_button", close_button_, 2);
50 
51  pub_ = nh_.advertise<pr2_controllers_msgs::Pr2GripperCommand>("command", 1, false);
52  sub_ = nh_.subscribe("joy", 1, &TeleopGripper::joyCallback, this);
53 
54  ROS_DEBUG("teleop_gripper started");
55  }
56 
57  void joyCallback(const sensor_msgs::JoyConstPtr& joy)
58  {
59  ROS_DEBUG("Got a joy msg");
60  if ((int) joy->buttons.size() <= open_button_ ||
61  (int) joy->buttons.size() <= close_button_ ||
62  open_button_ < 0 ||
63  close_button_ < 0)
64  {
65  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_);
66  return;
67  }
68 
69  ROS_DEBUG("open: Buttons[%u] = %u", open_button_, joy->buttons[open_button_]);
70 
71  if (joy->buttons[open_button_] == 1)
72  {
74  ROS_DEBUG("Opening");
75  }
76  else if (joy->buttons[close_button_] == 1)
77  {
79  ROS_DEBUG("Closing");
80  }
81  }
82 
83 private:
87 
90 
91 
92  pr2_controllers_msgs::Pr2GripperCommand open_cmd_;
93  pr2_controllers_msgs::Pr2GripperCommand close_cmd_;
94 };
95 
96 int main(int argc, char** argv)
97 {
98  ros::init(argc, argv, "teleop_gripper");
99 
100  TeleopGripper teleop_gripper;
101 
102  ros::spin();
103 
104 }
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher pub_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::NodeHandle nh_
ROSCPP_DECL void spin(Spinner &spinner)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Subscriber sub_
pr2_controllers_msgs::Pr2GripperCommand open_cmd_
#define ROS_ERROR(...)
pr2_controllers_msgs::Pr2GripperCommand close_cmd_
void joyCallback(const sensor_msgs::JoyConstPtr &joy)
#define ROS_DEBUG(...)


pr2_teleop
Author(s):
autogenerated on Thu Jun 6 2019 19:18:47