remote_control.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, PickNik Consulting
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 PickNik Consulting 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: Contains all hooks for debug interface
37 */
38 
39 // C++
40 #include <string>
41 
43 
44 #define CONSOLE_COLOR_RESET "\033[0m"
45 #define CONSOLE_COLOR_CYAN "\033[96m"
46 #define CONSOLE_COLOR_BROWN "\033[93m"
47 
48 namespace rviz_visual_tools
49 {
54 {
55  std::string rviz_dashboard_topic = "/rviz_visual_tools_gui";
56 
57  // Subscribe to Rviz Dashboard
58  const std::size_t button_queue_size = 10;
59  rviz_dashboard_sub_ = nh_.subscribe<sensor_msgs::Joy>(rviz_dashboard_topic, button_queue_size,
61 
62  ROS_INFO_STREAM_NAMED(name_, "RemoteControl Ready.");
63 }
64 
65 void RemoteControl::rvizDashboardCallback(const sensor_msgs::Joy::ConstPtr& msg)
66 {
67  if (msg->buttons[1] != 0)
68  {
70  }
71  else if (msg->buttons[2] != 0)
72  {
73  setAutonomous();
74  }
75  else if (msg->buttons[3] != 0)
76  {
78  }
79  else if (msg->buttons[4] != 0)
80  {
81  setStop();
82  }
83  else
84  {
85  ROS_ERROR_STREAM_NAMED(name_, "Unknown input button");
86  }
87 }
88 
90 {
91  stop_ = false;
92 
93  if (is_waiting_)
94  {
95  next_step_ready_ = true;
96  }
97  return true;
98 }
99 
100 void RemoteControl::setAutonomous(bool autonomous)
101 {
102  autonomous_ = autonomous;
103  stop_ = false;
104 }
105 
106 void RemoteControl::setFullAutonomous(bool autonomous)
107 {
108  full_autonomous_ = autonomous;
109  autonomous_ = autonomous;
110  stop_ = false;
111 }
112 
113 void RemoteControl::setStop(bool stop)
114 {
115  stop_ = stop;
116  if (stop)
117  {
118  autonomous_ = false;
119  full_autonomous_ = false;
120  }
121 }
122 
124 {
125  return stop_;
126 }
127 
129 {
130  return autonomous_;
131 }
132 
134 {
135  return full_autonomous_;
136 }
137 
138 bool RemoteControl::waitForNextStep(const std::string& caption)
139 {
140  // Check if we really need to wait
141  if (!(!next_step_ready_ && !autonomous_ && ros::ok()))
142  {
143  return true;
144  }
145 
146  // Show message
147  std::cout << std::endl;
148  std::cout << CONSOLE_COLOR_CYAN << "Waiting to continue: " << caption << CONSOLE_COLOR_RESET << std::flush;
149 
151  {
152  displayWaitingState_(true);
153  }
154 
155  is_waiting_ = true;
156  // Wait until next step is ready
157  while (!next_step_ready_ && !autonomous_ && ros::ok())
158  {
159  ros::Duration(0.25).sleep();
160  ros::spinOnce();
161  }
162  if (!ros::ok())
163  {
164  exit(0);
165  }
166 
167  next_step_ready_ = false;
168  is_waiting_ = false;
169  std::cout << CONSOLE_COLOR_CYAN << "... continuing" << CONSOLE_COLOR_RESET << std::endl;
170 
172  {
173  displayWaitingState_(false);
174  }
175  return true;
176 }
177 
178 bool RemoteControl::waitForNextFullStep(const std::string& caption)
179 {
180  // Check if we really need to wait
181  if (!(!next_step_ready_ && !full_autonomous_ && ros::ok()))
182  {
183  return true;
184  }
185 
186  // Show message
187  std::cout << std::endl;
188  std::cout << CONSOLE_COLOR_CYAN << "Waiting to continue: " << caption << CONSOLE_COLOR_RESET << std::flush;
189 
191  {
192  displayWaitingState_(true);
193  }
194 
195  is_waiting_ = true;
196  // Wait until next step is ready
197  while (!next_step_ready_ && !full_autonomous_ && ros::ok())
198  {
199  ros::Duration(0.25).sleep();
200  ros::spinOnce();
201  }
202  if (!ros::ok())
203  {
204  exit(0);
205  }
206 
207  next_step_ready_ = false;
208  is_waiting_ = false;
209  std::cout << CONSOLE_COLOR_CYAN << "... continuing" << CONSOLE_COLOR_RESET << std::endl;
210 
212  {
213  displayWaitingState_(false);
214  }
215  return true;
216 }
217 
218 } // namespace rviz_visual_tools
void setAutonomous(bool autonomous=true)
Enable autonomous mode.
#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 getStop()
See if we are in stop mode.
bool setReadyForNextStep()
Step to next step.
bool sleep() const
#define ROS_INFO_STREAM_NAMED(name, args)
bool waitForNextFullStep(const std::string &caption="go to next full step")
void rvizDashboardCallback(const sensor_msgs::Joy::ConstPtr &msg)
Callback from ROS topic.
void setStop(bool stop=true)
Stop something in pipeline.
#define CONSOLE_COLOR_RESET
ROSCPP_DECL bool ok()
DisplayWaitingState displayWaitingState_
RemoteControl(const ros::NodeHandle &nh)
Constructor.
bool getAutonomous()
Get the autonomous mode.
bool waitForNextStep(const std::string &caption="go to next step")
Wait until user presses a button.
#define CONSOLE_COLOR_CYAN
void setFullAutonomous(bool autonomous=true)
ROSCPP_DECL void spinOnce()


rviz_visual_tools
Author(s): Dave Coleman
autogenerated on Mon Feb 25 2019 03:54:12