main.cpp
Go to the documentation of this file.
1 /*******************************************************************************
2 * Copyright 2018 ROBOTIS CO., LTD.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *******************************************************************************/
16 
17 /*
18  * main.cpp
19  *
20  * Created on: 2016. 12. 16.
21  * Author: Jay Song
22  */
23 
24 #include "thormang3_action_editor/action_editor.h"
25 
26 void sighandler(int sig)
27 {
28  struct termios term;
29  tcgetattr( STDIN_FILENO, &term);
30  term.c_lflag |= ICANON | ECHO;
31  tcsetattr( STDIN_FILENO, TCSANOW, &term);
32 
33  exit(0);
34 }
35 
36 int main(int argc, char **argv)
37 {
38  ros::init(argc, argv, "THORMANG3 Action Editor");
39  ros::NodeHandle nh;
40 
41  std::string offset_file = nh.param<std::string>("offset_table", "");
42  std::string robot_file = nh.param<std::string>("robot_file_path", "");
43  std::string dxl_init_file = nh.param<std::string>("init_file_path", "");
44 
45  signal(SIGABRT, &sighandler);
46  signal(SIGTERM, &sighandler);
47  signal(SIGQUIT, &sighandler);
48  signal(SIGINT, &sighandler);
49 
50  int ch;
51 
52  thormang3::ActionEditor editor;
53  if (editor.initializeActionEditor(robot_file, dxl_init_file, offset_file) == false)
54  {
55  ROS_ERROR("Failed to Initialize");
56  return 0;
57  }
58 
59  editor.drawIntro();
60 
61  while (1)
62  {
63  ch = editor._getch();
64 
65  if (ch == 0x1b)
66  {
67  ch = editor._getch();
68  if (ch == 0x5b)
69  {
70  ch = editor._getch();
71  if (ch == 0x41) // Up arrow key
72  editor.moveUpCursor();
73  else if (ch == 0x42) // Down arrow key
74  editor.moveDownCursor();
75  else if (ch == 0x44) // Left arrow key
76  editor.moveLeftCursor();
77  else if (ch == 0x43) // Right arrow key
78  editor.moveRightCursor();
79  }
80  }
81  else if (ch == '[')
82  editor.setValueUpDown(-1);
83  else if (ch == ']')
84  editor.setValueUpDown(1);
85  else if (ch == '{')
86  editor.setValueUpDown(-10);
87  else if (ch == '}')
88  editor.setValueUpDown(10);
89  else if (ch == ' ')
90  editor.toggleTorque();
91  else if (ch == ',')
92  editor.storeValueToCache();
93  else if (ch == '.')
94  editor.setValueFromCache();
95  else if (ch == '/')
96  editor.clearCache();
97  else if (ch >= 'A' && ch <= 'z')
98  {
99  char input[128] = { 0, };
100  char *token;
101  int input_len;
102  char cmd[80];
103  int num_param;
104  int iparam[30];
105 
106  int idx = 0;
107 
108  editor.beginCommandMode();
109 
110  printf("%c", ch);
111  input[idx++] = (char) ch;
112 
113  while (1)
114  {
115  ch = editor._getch();
116  if (ch == 0x0A)
117  break;
118  else if (ch == 0x7F)
119  {
120  if (idx > 0)
121  {
122  ch = 0x08;
123  printf("%c", ch);
124  ch = ' ';
125  printf("%c", ch);
126  ch = 0x08;
127  printf("%c", ch);
128  input[--idx] = 0;
129  }
130  }
131  else if ((ch >= 'A' && ch <= 'z') || ch == ' ' || (ch >= '0' && ch <= '9'))
132  {
133  if (idx < 127)
134  {
135  printf("%c", ch);
136  input[idx++] = (char) ch;
137  }
138  }
139  }
140 
141  fflush(stdin);
142  input_len = strlen(input);
143  if (input_len > 0)
144  {
145  token = strtok(input, " ");
146  if (token != 0)
147  {
148  strcpy(cmd, token);
149  token = strtok(0, " ");
150  num_param = 0;
151  while (token != 0)
152  {
153  iparam[num_param++] = atoi(token);
154  token = strtok(0, " ");
155  }
156 
157  if (strcmp(cmd, "exit") == 0)
158  {
159  if (editor.askSave() == false)
160  break;
161  }
162  else if (strcmp(cmd, "re") == 0)
163  editor.drawPage();
164  else if (strcmp(cmd, "help") == 0)
165  editor.helpCmd();
166  else if (strcmp(cmd, "n") == 0)
167  editor.nextCmd();
168  else if (strcmp(cmd, "b") == 0)
169  editor.prevCmd();
170  else if (strcmp(cmd, "time") == 0)
171  editor.timeCmd();
172  else if (strcmp(cmd, "speed") == 0)
173  editor.speedCmd();
174  else if (strcmp(cmd, "page") == 0)
175  {
176  if (num_param > 0)
177  editor.pageCmd(iparam[0]);
178  else
179  editor.printCmd("Need parameter");
180  }
181  else if (strcmp(cmd, "play") == 0)
182  {
183  editor.playCmd();
184  }
185  else if (strcmp(cmd, "playboth") == 0)
186  {
187  if (num_param > 0)
188  editor.playCmd(iparam[0]);
189  else
190  editor.printCmd("Need parameter");
191  }
192  else if (strcmp(cmd, "set") == 0)
193  {
194  if (num_param > 0)
195  editor.setValue(iparam[0]);
196  else
197  editor.printCmd("Need parameter");
198  }
199  else if (strcmp(cmd, "list") == 0)
200  editor.listCmd();
201  else if (strcmp(cmd, "on") == 0)
202  editor.turnOnOffCmd(true, num_param, iparam);
203  else if (strcmp(cmd, "off") == 0)
204  editor.turnOnOffCmd(false, num_param, iparam);
205  else if (strcmp(cmd, "mrl") == 0)
206  {
207  if (num_param > 0)
208  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::RightToLeft,
209  thormang3::ActionEditor::AllBody);
210  else
211  editor.printCmd("Need parameter");
212  }
213  else if (strcmp(cmd, "murl") == 0)
214  {
215  if (num_param > 0)
216  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::RightToLeft,
217  thormang3::ActionEditor::UpperBody);
218  else
219  editor.printCmd("Need parameter");
220  }
221  else if (strcmp(cmd, "mlrl") == 0)
222  {
223  if (num_param > 0)
224  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::RightToLeft,
225  thormang3::ActionEditor::LowerBody);
226  else
227  editor.printCmd("Need parameter");
228  }
229  else if (strcmp(cmd, "mlr") == 0)
230  {
231  if (num_param > 0)
232  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::LeftToRight,
233  thormang3::ActionEditor::AllBody);
234  else
235  editor.printCmd("Need parameter");
236  }
237  else if (strcmp(cmd, "mulr") == 0)
238  {
239  if (num_param > 0)
240  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::LeftToRight,
241  thormang3::ActionEditor::UpperBody);
242  else
243  editor.printCmd("Need parameter");
244  }
245  else if (strcmp(cmd, "mllr") == 0)
246  {
247  if (num_param > 0)
248  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::LeftToRight,
249  thormang3::ActionEditor::LowerBody);
250  else
251  editor.printCmd("Need parameter");
252  }
253  else if (strcmp(cmd, "ms") == 0)
254  {
255  if (num_param > 0)
256  editor.mirrorStepCmd(iparam[0], thormang3::ActionEditor::SwitchEach,
257  thormang3::ActionEditor::AllBody);
258  else
259  editor.printCmd("Need parameter");
260  }
261  else if (strcmp(cmd, "w") == 0)
262  {
263  if (num_param > 0)
264  editor.writeStepCmd(iparam[0]);
265  else
266  editor.printCmd("Need parameter");
267  }
268  else if (strcmp(cmd, "d") == 0)
269  {
270  if (num_param > 0)
271  editor.deleteStepCmd(iparam[0]);
272  else
273  editor.printCmd("Need parameter");
274  }
275  else if (strcmp(cmd, "i") == 0)
276  {
277  if (num_param == 0)
278  editor.insertStepCmd(0);
279  else
280  editor.insertStepCmd(iparam[0]);
281  }
282  else if (strcmp(cmd, "copy") == 0)
283  {
284  if (num_param > 0)
285  editor.copyCmd(iparam[0]);
286  else
287  editor.printCmd("Need parameter");
288  }
289  else if (strcmp(cmd, "new") == 0)
290  editor.newCmd();
291  else if (strcmp(cmd, "g") == 0)
292  {
293  if (num_param > 0)
294  editor.goCmd(iparam[0]);
295  else
296  editor.printCmd("Need parameter");
297  }
298  else if (strcmp(cmd, "save") == 0)
299  editor.saveCmd();
300  else if (strcmp(cmd, "name") == 0)
301  editor.nameCmd();
302  else
303  editor.printCmd("Bad command! please input 'help'");
304  }
305  }
306 
307  editor.endCommandMode();
308  }
309  }
310 
311  editor.drawEnding();
312 
313  return 0;
314  }
thormang3::ActionEditor::deleteStepCmd
void deleteStepCmd(int index)
Definition: action_editor.cpp:1538
ros::NodeHandle
thormang3::ActionEditor::initializeActionEditor
bool initializeActionEditor(std::string robot_file_path, std::string init_file_path, std::string offset_file_path)
Definition: action_editor.cpp:259
cmd
string cmd
main
int main(int argc, char **argv)
Definition: main.cpp:36
thormang3::ActionEditor::writeStepCmd
void writeStepCmd(int index)
Definition: action_editor.cpp:1516
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
thormang3::ActionEditor::setValueUpDown
void setValueUpDown(int offset)
Definition: action_editor.cpp:808
thormang3::ActionEditor::printCmd
void printCmd(const char *message)
Definition: action_editor.cpp:779
thormang3::ActionEditor::insertStepCmd
void insertStepCmd(int index)
Definition: action_editor.cpp:1577
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
thormang3::ActionEditor::mirrorStepCmd
void mirrorStepCmd(int index, int mirror_type, int target_type)
Definition: action_editor.cpp:1440
thormang3::ActionEditor::turnOnOffCmd
void turnOnOffCmd(bool on, int num_param, int *list)
Definition: action_editor.cpp:1405
sighandler
void sighandler(int sig)
Definition: main.cpp:26
thormang3::ActionEditor::setValue
void setValue(int value)
Definition: action_editor.cpp:814
ROS_ERROR
#define ROS_ERROR(...)
action_editor.h

thormang3_action_editor
Author(s): Kayman , SCH , Jay Song
autogenerated on Mon Jun 10 2019 15:37:46