roch_joy.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 #include <geometry_msgs/Twist.h>
32 #include <sensor_msgs/Joy.h>
33 #include "boost/thread/mutex.hpp"
34 #include "boost/thread/thread.hpp"
35 #include "ros/console.h"
36 
38 {
39 public:
40  RochTeleop();
41 
42 private:
43  void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
44  void publish();
45 
47 
49  double l_scale_, a_scale_;
52 
53  geometry_msgs::Twist last_published_;
54  boost::mutex publish_mutex_;
58 
59 };
60 
62  ph_("~"),
63  linear_(1),
64  angular_(0),
65  deadman_axis_(4),
66  l_scale_(0.3),
67  a_scale_(0.9)
68 {
69  ph_.param("axis_linear", linear_, linear_);
70  ph_.param("axis_angular", angular_, angular_);
71  ph_.param("axis_deadman", deadman_axis_, deadman_axis_);
72  ph_.param("scale_angular", a_scale_, a_scale_);
73  ph_.param("scale_linear", l_scale_, l_scale_);
74 
75  deadman_pressed_ = false;
76  zero_twist_published_ = false;
77 
78  vel_pub_ = ph_.advertise<geometry_msgs::Twist>("cmd_vel", 1, true);
79  joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &RochTeleop::joyCallback, this);
80 
81  timer_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&RochTeleop::publish, this));
82 }
83 
84 void RochTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
85 {
86  geometry_msgs::Twist vel;
87  vel.angular.z = a_scale_*joy->axes[angular_];
88  vel.linear.x = l_scale_*joy->axes[linear_];
89  last_published_ = vel;
90  deadman_pressed_ = joy->buttons[deadman_axis_];
91 }
92 
94 {
95  boost::mutex::scoped_lock lock(publish_mutex_);
96 
97  if (deadman_pressed_)
98  {
100  zero_twist_published_=false;
101  }
103  {
104  vel_pub_.publish(*new geometry_msgs::Twist());
106  }
107 }
108 
109 int main(int argc, char** argv)
110 {
111  ros::init(argc, argv, "roch_teleop");
112  RochTeleop roch_teleop;
113 
114  ros::spin();
115 }
ros::Subscriber joy_sub_
Definition: roch_joy.cpp:51
double l_scale_
Definition: roch_joy.cpp:49
void publish(const boost::shared_ptr< M > &message) const
int angular_
Definition: roch_joy.cpp:48
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool zero_twist_published_
Definition: roch_joy.cpp:56
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
geometry_msgs::Twist last_published_
Definition: roch_joy.cpp:53
ros::NodeHandle nh_
Definition: roch_joy.cpp:46
void joyCallback(const sensor_msgs::Joy::ConstPtr &joy)
Definition: roch_joy.cpp:84
ros::Timer timer_
Definition: roch_joy.cpp:57
ROSCPP_DECL void spin(Spinner &spinner)
boost::mutex publish_mutex_
Definition: roch_joy.cpp:54
int deadman_axis_
Definition: roch_joy.cpp:48
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool deadman_pressed_
Definition: roch_joy.cpp:55
ros::NodeHandle ph_
Definition: roch_joy.cpp:46
ros::Publisher vel_pub_
Definition: roch_joy.cpp:50
int main(int argc, char **argv)
Definition: roch_joy.cpp:109
double a_scale_
Definition: roch_joy.cpp:49
int linear_
Definition: roch_joy.cpp:48
void publish()
Definition: roch_joy.cpp:93


roch_teleop
Author(s): Chen
autogenerated on Mon Jun 10 2019 14:44:16