joystick_manual_control.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman <dave@dav.ee>
36  Desc: Inherit from this file to enable joystick mode switching of your robot
37 */
38 
39 #ifndef ROS_CONTROL_BOILERPLATE__JOYSTICK_MANUAL_CONTROL
40 #define ROS_CONTROL_BOILERPLATE__JOYSTICK_MANUAL_CONTROL
41 
42 // ROS
43 #include <ros/ros.h>
44 #include <sensor_msgs/Joy.h>
45 
46 // ros_control
47 #include <controller_manager_msgs/SwitchController.h>
48 #include <controller_manager_msgs/LoadController.h>
49 
51 {
53 {
54 public:
61  JoystickManualControl(const std::string& parent_name, const std::string& service_namespace)
62  : parent_name_(parent_name)
64  {
65  switch_service_ = service_namespace + "/controller_manager/switch_controller";
66  load_service_ = service_namespace + "/controller_manager/load_controller";
67 
68  // Switch modes of controllers
70  nh_.serviceClient<controller_manager_msgs::SwitchController>(switch_service_);
72  nh_.serviceClient<controller_manager_msgs::LoadController>(load_service_);
73 
74  // Subscribe to joystick control
75  std::size_t queue_size = 1;
76  remote_joy_ = nh_.subscribe("/joy", queue_size, &JoystickManualControl::joyCallback, this);
77 
78  ROS_INFO_STREAM_NAMED(parent_name_, "JoystickManualControl Ready.");
79  }
80 
85  virtual void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) = 0;
86 
91  {
92  // Ensure services are up
93  ROS_INFO_STREAM_NAMED(parent_name_, "Waiting for serivces...");
95  ROS_ERROR_STREAM_NAMED(parent_name_, "Unable to find service " << switch_service_);
97  ROS_ERROR_STREAM_NAMED(parent_name_, "Unable to find service " << load_service_);
98 
99  for (std::size_t i = 0; i < manual_controllers_.size(); ++i)
100  {
101  ROS_INFO_STREAM_NAMED(parent_name_, "Loading controller " << manual_controllers_[i]);
102  controller_manager_msgs::LoadController service;
103  service.request.name = manual_controllers_[i];
104  std::size_t counter = 0;
105  while (!load_controlers_client_.call(service) && ros::ok())
106  {
107  if (counter > 100)
108  ROS_WARN_STREAM_THROTTLE_NAMED(1.0, parent_name_, "Failed to load controller '"
109  << manual_controllers_[i]
110  << "', trying again");
111  ros::spinOnce();
112  ros::Duration(0.1).sleep();
113  counter++;
114  }
115  }
116 
117  return true;
118  }
119 
121  {
122  // Stop all controllers, soft E-Stop
123  controller_manager_msgs::SwitchController service;
124  service.request.strictness = service.request.STRICT;
125 
126  ROS_WARN_STREAM_NAMED(parent_name_, "Switching to MANUAL control");
127  for (std::size_t i = 0; i < manual_controllers_.size(); ++i)
128  {
129  service.request.start_controllers.push_back(manual_controllers_[i]);
130  }
131  for (std::size_t i = 0; i < trajectory_controllers_.size(); ++i)
132  {
133  service.request.stop_controllers.push_back(trajectory_controllers_[i]);
134  }
135 
136  // Attempt stop
137  if (!switch_controlers_client_.call(service))
138  {
139  ROS_ERROR_STREAM_NAMED(parent_name_, "Failed to switch controllers");
140  return;
141  }
142  }
143 
145  {
146  // Stop all controllers, soft E-Stop
147  controller_manager_msgs::SwitchController service;
148  service.request.strictness = service.request.STRICT;
149 
150  ROS_INFO_STREAM_NAMED(parent_name_, "Switching to TRAJECTORY control");
151  for (std::size_t i = 0; i < manual_controllers_.size(); ++i)
152  {
153  service.request.stop_controllers.push_back(manual_controllers_[i]);
154  }
155  for (std::size_t i = 0; i < trajectory_controllers_.size(); ++i)
156  {
157  service.request.start_controllers.push_back(trajectory_controllers_[i]);
158  }
159 
160  // Attempt stop
161  if (!switch_controlers_client_.call(service))
162  {
163  ROS_ERROR_STREAM_NAMED(parent_name_, "Failed to switch controllers");
164  return;
165  }
166  }
167 
168 protected:
169  // A shared node handle
171 
172  // Name of parent class, used for logging messages
173  const std::string parent_name_;
174  std::string switch_service_;
175  std::string load_service_;
176 
177  // Subscribe to joystick commands
179 
180  // Ability to switch controllers
183 
184  // Switching controller mode
186 
187  // Controller lists
188  std::vector<std::string> manual_controllers_;
189  std::vector<std::string> trajectory_controllers_;
190 
191 }; // end class
192 
193 } // end namespace
194 
195 #endif
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define ROS_ERROR_STREAM_NAMED(name, args)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
bool call(MReq &req, MRes &res)
virtual void joyCallback(const sensor_msgs::Joy::ConstPtr &msg)=0
Response to joystick control Button mapping is customized by each class that inherits from this...
#define ROS_INFO_STREAM_NAMED(name, args)
JoystickManualControl(const std::string &parent_name, const std::string &service_namespace)
Constructor.
#define ROS_WARN_STREAM_THROTTLE_NAMED(rate, name, args)
ROSCPP_DECL bool ok()
ROSCPP_DECL void spinOnce()
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
#define ROS_WARN_STREAM_NAMED(name, args)
bool loadManualControllers()
Load a secondary manual controller.


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Thu Feb 25 2021 03:58:54