Apollo 10.0
自动驾驶开放平台
teleop.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
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#include <termios.h>
18
19#include <cstdio>
20#include <iostream>
21#include <memory>
22#include <thread>
23
24#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
25#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
26
27#include "cyber/common/log.h"
28#include "cyber/common/macros.h"
29#include "cyber/cyber.h"
30#include "cyber/init.h"
31#include "cyber/time/time.h"
32#include "modules/canbus/vehicle/vehicle_controller.h"
33#include "modules/common/adapters/adapter_gflags.h"
34#include "modules/common/configs/vehicle_config_helper.h"
35#include "modules/common/util/message_util.h"
36
37// gflags
38DEFINE_double(throttle_inc_delta, 2.0,
39 "throttle pedal command delta percentage.");
40DEFINE_double(brake_inc_delta, 2.0, "brake pedal delta percentage");
41DEFINE_double(steer_inc_delta, 2.0, "steer delta percentage");
42// TODO(ALL) : switch the acceleration cmd or pedal cmd
43// default : use pedal cmd
44DEFINE_bool(
45 use_acceleration, false,
46 "switch to use acceleration instead of throttle pedal and brake pedal");
47
48namespace {
49
50using apollo::canbus::Chassis;
51using apollo::common::VehicleSignal;
52using apollo::control::ControlCommand;
53using apollo::control::PadMessage;
54using apollo::cyber::CreateNode;
55using apollo::cyber::Reader;
56using apollo::cyber::Time;
57using apollo::cyber::Writer;
58
59const uint32_t KEYCODE_O = 0x4F; // 'O' not number
60
61const uint32_t KEYCODE_UP1 = 0x57; // 'W'
62const uint32_t KEYCODE_UP2 = 0x77; // 'w'
63const uint32_t KEYCODE_DN1 = 0x53; // 'S'
64const uint32_t KEYCODE_DN2 = 0x73; // 's'
65const uint32_t KEYCODE_LF1 = 0x41; // 'A'
66const uint32_t KEYCODE_LF2 = 0x61; // 'a'
67const uint32_t KEYCODE_RT1 = 0x44; // 'D'
68const uint32_t KEYCODE_RT2 = 0x64; // 'd'
69
70// set hand brake or parking brake
71const uint32_t KEYCODE_PKBK1 = 0x50; // 'P'
72const uint32_t KEYCODE_PKBK2 = 0x70; // 'p'
73
74// set throttle, gear, and brake
75const uint32_t KEYCODE_SETT1 = 0x54; // 'T'
76const uint32_t KEYCODE_SETT2 = 0x74; // 't'
77const uint32_t KEYCODE_SETG1 = 0x47; // 'G'
78const uint32_t KEYCODE_SETG2 = 0x67; // 'g'
79const uint32_t KEYCODE_SETB1 = 0x42; // 'B'
80const uint32_t KEYCODE_SETB2 = 0x62; // 'b'
81const uint32_t KEYCODE_ZERO = 0x30; // '0' number
82
83// set turn signal
84const uint32_t KEYCODE_SETQ1 = 0x51; // 'Q'
85const uint32_t KEYCODE_SETQ2 = 0x71; // 'q'
86
87// set low beam
88const uint32_t KEYCODE_SETL1 = 0x4C; // 'L'
89const uint32_t KEYCODE_SETL2 = 0x6C; // 'l'
90
91// change action
92const uint32_t KEYCODE_MODE1 = 0x4D; // 'M'
93const uint32_t KEYCODE_MODE2 = 0x6D; // 'm'
94
95// emergency stop
96const uint32_t KEYCODE_ESTOP1 = 0x45; // 'E'
97const uint32_t KEYCODE_ESTOP2 = 0x65; // 'e'
98
99// help
100const uint32_t KEYCODE_HELP1 = 0x48; // 'H'
101const uint32_t KEYCODE_HELP2 = 0x68; // 'h'
102
103class Teleop {
104 public:
105 Teleop() {
106 ResetControlCommand();
107 node_ = CreateNode("teleop");
108 }
109 static void PrintKeycode() {
110 system("clear");
111 printf("===================== KEYBOARD MAP ===================\n");
112 printf("HELP: [%c] |\n", KEYCODE_HELP2);
113 printf("Set Action: [%c]+Num\n", KEYCODE_MODE2);
114 printf(" 0 RESET ACTION\n");
115 printf(" 1 START ACTION\n");
116 printf(" 2 VIN_REQ ACTION\n");
117 printf("\n-----------------------------------------------------------\n");
118 printf("Set Gear: [%c]+Num\n", KEYCODE_SETG2);
119 printf(" 0 GEAR_NEUTRAL\n");
120 printf(" 1 GEAR_DRIVE\n");
121 printf(" 2 GEAR_REVERSE\n");
122 printf(" 3 GEAR_PARKING\n");
123 printf(" 4 GEAR_LOW\n");
124 printf(" 5 GEAR_INVALID\n");
125 printf(" 6 GEAR_NONE\n");
126 printf("\n-----------------------------------------------------------\n");
127 printf("Throttle/Speed up: [%c] | \n", KEYCODE_UP2);
128 printf("Brake/Speed down: [%c] | \n", KEYCODE_DN2);
129 printf("Steer LEFT: [%c] | Steer RIGHT: [%c]\n",
130 KEYCODE_LF2, KEYCODE_RT2);
131 printf("Parking Brake: [%c] | Emergency Stop [%c]\n",
132 KEYCODE_PKBK2, KEYCODE_ESTOP2);
133 printf("Left/Right Lamp: [%c] | Low beam [%c]\n",
134 KEYCODE_SETQ2, KEYCODE_SETL2);
135 printf("\n-----------------------------------------------------------\n");
136 printf("Exit: Ctrl + C, then press enter to normal terminal\n");
137 printf("===========================================================\n");
138 }
139
140 void KeyboardLoopThreadFunc() {
141 char c = 0;
142 int32_t level = 0;
143 double brake = 0;
144 double throttle = 0;
145 double acc = 0;
146 double dec = 0;
147 double steering = 0;
148 struct termios cooked_;
149 struct termios raw_;
150 int32_t kfd_ = 0;
151 bool parking_brake = false;
152 bool low_beam = false;
153 Chassis::GearPosition gear = Chassis::GEAR_INVALID;
154 PadMessage pad_msg;
155 ControlCommand &control_command_ = control_command();
156 apollo::common::VehicleParam vehicle_params_ =
157 apollo::common::VehicleConfigHelper::Instance()
158 ->GetConfig()
159 .vehicle_param();
160
161 // get the console in raw mode
162 tcgetattr(kfd_, &cooked_);
163 std::memcpy(&raw_, &cooked_, sizeof(struct termios));
164 raw_.c_lflag &= ~(ICANON | ECHO);
165 // Setting a new line, then end of file
166 raw_.c_cc[VEOL] = 1;
167 raw_.c_cc[VEOF] = 2;
168 tcsetattr(kfd_, TCSANOW, &raw_);
169 puts("Teleop:\nReading from keyboard now.");
170 puts("---------------------------");
171 puts("Use arrow keys to drive the car.");
172 while (IsRunning()) {
173 // get the next event from the keyboard
174 if (read(kfd_, &c, 1) < 0) {
175 perror("read():");
176 exit(-1);
177 }
178 AINFO << "control command : "
179 << control_command_.ShortDebugString().c_str();
180 switch (c) {
181 case KEYCODE_UP1: // accelerate
182 case KEYCODE_UP2:
183 if (!FLAGS_use_acceleration) {
184 brake = control_command_.brake();
185 throttle = control_command_.throttle();
186 }
187 if (brake > 1e-6) {
188 brake = GetCommand(brake, -FLAGS_brake_inc_delta);
189 if (!FLAGS_use_acceleration) {
190 control_command_.set_brake(brake);
191 } else {
192 dec = brake / 100 * vehicle_params_.max_deceleration();
193 control_command_.set_acceleration(dec);
194 }
195 } else {
196 throttle = GetCommand(throttle, FLAGS_throttle_inc_delta);
197 if (!FLAGS_use_acceleration) {
198 control_command_.set_throttle(throttle);
199 } else {
200 acc = throttle / 100 * vehicle_params_.max_acceleration();
201 control_command_.set_acceleration(acc);
202 }
203 }
204 if (!FLAGS_use_acceleration) {
205 AINFO << "Throttle = " << control_command_.throttle()
206 << ", Brake = " << control_command_.brake();
207 } else {
208 AINFO << "Acceleration = " << control_command_.acceleration();
209 }
210 break;
211 case KEYCODE_DN1: // decelerate
212 case KEYCODE_DN2:
213 if (!FLAGS_use_acceleration) {
214 brake = control_command_.brake();
215 throttle = control_command_.throttle();
216 }
217 if (throttle > 1e-6) {
218 throttle = GetCommand(throttle, -FLAGS_throttle_inc_delta);
219 if (!FLAGS_use_acceleration) {
220 control_command_.set_throttle(throttle);
221 } else {
222 acc = throttle / 100 * vehicle_params_.max_acceleration();
223 control_command_.set_acceleration(acc);
224 }
225 } else {
226 brake = GetCommand(brake, FLAGS_brake_inc_delta);
227 if (!FLAGS_use_acceleration) {
228 control_command_.set_brake(brake);
229 } else {
230 dec = brake / 100 * vehicle_params_.max_deceleration();
231 control_command_.set_acceleration(dec);
232 }
233 }
234 if (!FLAGS_use_acceleration) {
235 AINFO << "Throttle = " << control_command_.throttle()
236 << ", Brake = " << control_command_.brake();
237 } else {
238 AINFO << "Acceleration = " << control_command_.acceleration();
239 }
240 break;
241 case KEYCODE_LF1: // steer left
242 case KEYCODE_LF2:
243 steering = control_command_.steering_target();
244 steering = GetCommand(steering, FLAGS_steer_inc_delta);
245 control_command_.set_steering_target(steering);
246 AINFO << "Steering Target = " << steering;
247 break;
248 case KEYCODE_RT1: // steer right
249 case KEYCODE_RT2:
250 steering = control_command_.steering_target();
251 steering = GetCommand(steering, -FLAGS_steer_inc_delta);
252 control_command_.set_steering_target(steering);
253 AINFO << "Steering Target = " << steering;
254 break;
255 case KEYCODE_PKBK1: // hand brake
256 case KEYCODE_PKBK2:
257 parking_brake = !control_command_.parking_brake();
258 control_command_.set_parking_brake(parking_brake);
259 AINFO << "Parking Brake Toggled: " << parking_brake;
260 break;
261 case KEYCODE_ESTOP1:
262 case KEYCODE_ESTOP2:
263 control_command_.set_brake(50.0);
264 AINFO << "Estop Brake : " << control_command_.brake();
265 break;
266 // case KEYCODE_SETT1: // set throttle 0-50
267 // case KEYCODE_SETT2:
268 // // read keyboard again
269 // if (read(kfd_, &c, 1) < 0) {
270 // exit(-1);
271 // }
272 // level = c - KEYCODE_ZERO;
273 // control_command_.set_throttle(level * 5.0);
274 // control_command_.set_brake(0.0);
275 // AINFO << "Throttle = " << control_command_.throttle()
276 // << ", Brake = " << control_command_.brake();
277 // break;
278 case KEYCODE_SETG1: // set gear
279 case KEYCODE_SETG2:
280 // read keyboard again
281 if (read(kfd_, &c, 1) < 0) {
282 exit(-1);
283 }
284 level = c - KEYCODE_ZERO;
285 gear = GetGear(level);
286 control_command_.set_gear_location(gear);
287 AINFO << "Gear set to : " << level;
288 break;
289 // case KEYCODE_SETB1: // set brake 0-50
290 // case KEYCODE_SETB2:
291 // // read keyboard again
292 // if (read(kfd_, &c, 1) < 0) {
293 // exit(-1);
294 // }
295 // level = c - KEYCODE_ZERO;
296 // control_command_.set_throttle(0.0);
297 // control_command_.set_brake(level * 5.0);
298 // AINFO << "Throttle = " << control_command_.throttle()
299 // << ", Brake = " << control_command_.brake();
300 // break;
301 case KEYCODE_SETQ1: // set turn signal
302 case KEYCODE_SETQ2:
303 static int cnt = 0;
304 ++cnt;
305 if (cnt > 3) {
306 cnt = 0;
307 }
308 if (cnt == 0) {
309 control_command_.mutable_signal()->set_turn_signal(
310 VehicleSignal::TURN_NONE);
311 AINFO << "Set Lamp OFF";
312 } else if (cnt == 1) {
313 control_command_.mutable_signal()->set_turn_signal(
314 VehicleSignal::TURN_LEFT);
315 AINFO << "Set Turn Left";
316 } else if (cnt == 2) {
317 control_command_.mutable_signal()->set_turn_signal(
318 VehicleSignal::TURN_RIGHT);
319 AINFO << "Set Turn Right";
320 } else if (cnt == 3) {
321 control_command_.mutable_signal()->set_turn_signal(
322 VehicleSignal::TURN_HAZARD_WARNING);
323 AINFO << "Set Turn Hazard Warning Lamp ON";
324 }
325 break;
326 case KEYCODE_SETL1: // set low beam
327 case KEYCODE_SETL2:
328 low_beam = !control_command_.signal().low_beam();
329 control_command_.mutable_signal()->set_low_beam(low_beam);
330 AINFO << "Low Beam Toggled: " << low_beam;
331 break;
332 case KEYCODE_MODE1: // set action
333 case KEYCODE_MODE2:
334 // read keyboard again
335 if (read(kfd_, &c, 1) < 0) {
336 exit(-1);
337 }
338 level = c - KEYCODE_ZERO;
339 GetPadMessage(&pad_msg, level);
340 control_command_.mutable_pad_msg()->CopyFrom(pad_msg);
341 sleep(1);
342 control_command_.clear_pad_msg();
343 break;
344 case KEYCODE_HELP1: // set help
345 case KEYCODE_HELP2:
346 PrintKeycode();
347 break;
348 default:
349 // printf("%X\n", c);
350 break;
351 }
352 AINFO << "control command after switch : "
353 << control_command_.ShortDebugString().c_str();
354 } // keyboard_loop big while
355 tcsetattr(kfd_, TCSANOW, &cooked_);
356 AINFO << "keyboard_loop thread quited.";
357 return;
358 } // end of keyboard loop thread
359
360 ControlCommand &control_command() { return control_command_; }
361
362 Chassis::GearPosition GetGear(int32_t gear) {
363 switch (gear) {
364 case 0:
365 return Chassis::GEAR_NEUTRAL;
366 case 1:
367 return Chassis::GEAR_DRIVE;
368 case 2:
369 return Chassis::GEAR_REVERSE;
370 case 3:
371 return Chassis::GEAR_PARKING;
372 case 4:
373 return Chassis::GEAR_LOW;
374 case 5:
375 return Chassis::GEAR_INVALID;
376 case 6:
377 return Chassis::GEAR_NONE;
378 default:
379 return Chassis::GEAR_INVALID;
380 }
381 }
382
383 void GetPadMessage(PadMessage *pad_msg, int32_t int_action) {
384 apollo::control::DrivingAction action =
385 apollo::control::DrivingAction::RESET;
386 switch (int_action) {
387 case 0:
388 action = apollo::control::DrivingAction::RESET;
389 AINFO << "SET Action RESET";
390 break;
391 case 1:
392 action = apollo::control::DrivingAction::START;
393 AINFO << "SET Action START";
394 break;
395 case 2:
396 action = apollo::control::DrivingAction::VIN_REQ;
397 AINFO << "SET Action VIN_REQ";
398 break;
399 default:
400 AINFO << "unknown action: " << int_action << " use default RESET";
401 break;
402 }
403 pad_msg->set_action(action);
404 return;
405 }
406
407 double GetCommand(double val, double inc) {
408 val += inc;
409 if (val > 100.0) {
410 val = 100.0;
411 } else if (val < -100.0) {
412 val = -100.0;
413 }
414 return val;
415 }
416
417 void Send() {
418 apollo::common::util::FillHeader("control", &control_command_);
419 control_command_writer_->Write(control_command_);
420 ADEBUG << "Control Command send OK:" << control_command_.ShortDebugString();
421 }
422
423 void ResetControlCommand() {
424 control_command_.Clear();
425 control_command_.set_throttle(0.0);
426 control_command_.set_brake(0.0);
427 control_command_.set_steering_rate(0.0);
428 control_command_.set_steering_target(0.0);
429 control_command_.set_parking_brake(false);
430 control_command_.set_speed(0.0);
431 control_command_.set_acceleration(0.0);
432 control_command_.set_reset_model(false);
433 control_command_.set_engine_on_off(false);
434 control_command_.set_driving_mode(Chassis::COMPLETE_MANUAL);
435 control_command_.set_gear_location(Chassis::GEAR_INVALID);
436 control_command_.mutable_signal()->set_low_beam(false);
437 control_command_.mutable_signal()->set_turn_signal(
438 VehicleSignal::TURN_NONE);
439 }
440
441 void OnChassis(const Chassis &chassis) { Send(); }
442
443 int32_t Start() {
444 if (is_running_) {
445 AERROR << "Already running.";
446 return -1;
447 }
448 is_running_ = true;
449 chassis_reader_ = node_->CreateReader<Chassis>(
450 FLAGS_chassis_topic, [this](const std::shared_ptr<Chassis> &chassis) {
451 OnChassis(*chassis);
452 });
453 control_command_writer_ =
454 node_->CreateWriter<ControlCommand>(FLAGS_control_command_topic);
455 keyboard_thread_.reset(
456 new std::thread([this] { KeyboardLoopThreadFunc(); }));
457 if (keyboard_thread_ == nullptr) {
458 AERROR << "Unable to create can client receiver thread.";
459 return -1;
460 }
461 return 0;
462 }
463
464 void Stop() {
465 if (is_running_) {
466 is_running_ = false;
467 if (keyboard_thread_ != nullptr && keyboard_thread_->joinable()) {
468 keyboard_thread_->join();
469 keyboard_thread_.reset();
470 AINFO << "Teleop keyboard stopped [ok].";
471 }
472 }
473 }
474
475 bool IsRunning() const { return is_running_; }
476
477 private:
478 std::unique_ptr<std::thread> keyboard_thread_;
479 std::shared_ptr<Reader<Chassis>> chassis_reader_;
480 std::shared_ptr<Writer<ControlCommand>> control_command_writer_;
481 ControlCommand control_command_;
482 bool is_running_ = false;
483 std::shared_ptr<apollo::cyber::Node> node_;
484};
485
486} // namespace
487
488int main(int32_t argc, char **argv) {
489 apollo::cyber::Init(argv[0]);
490 FLAGS_alsologtostderr = true;
491 FLAGS_v = 3;
492
493 google::ParseCommandLineFlags(&argc, &argv, true);
494
495 Teleop teleop;
496
497 if (teleop.Start() != 0) {
498 AERROR << "Teleop start failed.";
499 return -1;
500 }
501 Teleop::PrintKeycode();
502 apollo::cyber::WaitForShutdown();
503 teleop.Stop();
504 AINFO << "Teleop exit done.";
505 return 0;
506}
adapter_gflags.h
DEFINE_double
DEFINE_double(throttle_inc_delta, 2.0, "throttle pedal command delta percentage.")
main
int main(int32_t argc, char **argv)
Definition teleop.cc:488
DEFINE_bool
DEFINE_bool(use_acceleration, false, "switch to use acceleration instead of throttle pedal and brake pedal")
apollo::cyber::Reader
Reader subscribes a channel, it has two main functions:
Definition reader.h:69
apollo::cyber::Time
Cyber has builtin time type Time.
Definition time.h:31
ADEBUG
#define ADEBUG
Definition log.h:41
AERROR
#define AERROR
Definition log.h:44
AINFO
#define AINFO
Definition log.h:42
message_util.h
Some string util functions.
apollo::cyber::WaitForShutdown
void WaitForShutdown()
Definition state.h:50
apollo::cyber::Init
bool Init(const char *binary_name, const std::string &dag_info)
Definition init.cc:98
apollo::cyber::CreateNode
std::unique_ptr< Node > CreateNode(const std::string &node_name, const std::string &name_space)
Definition cyber.cc:33
vehicle_config_helper.h
vehicle_controller.h
The class of VehicleController