|
14 | 14 |
|
15 | 15 | MotionDecision::MotionDecision(void) : private_nh_("~") |
16 | 16 | { |
17 | | - intersection_flag_pub_ = nh_.advertise<std_msgs::Bool>("/intersection_flag", 1, true); |
18 | | - cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1, true); |
19 | | - |
20 | | - emergency_stop_flag_sub_ = nh_.subscribe("/emergency_stop", 1, &MotionDecision::emergency_stop_flag_callback, this); |
21 | | - front_laser_sub_ = nh_.subscribe("/front_laser/scan", 1, &MotionDecision::front_laser_callback, this); |
22 | | - joy_sub_ = nh_.subscribe("/joy", 1, &MotionDecision::joy_callback, this); |
23 | | - local_path_cmd_vel_sub_ = nh_.subscribe("/local_path/cmd_vel", 1, &MotionDecision::local_path_cmd_vel_callback, this); |
24 | | - local_map_sub_ = nh_.subscribe("/local_map", 1, &MotionDecision::local_map_callback, this); |
25 | | - odom_sub_ = nh_.subscribe("/odom", 1, &MotionDecision::odom_callback, this); |
26 | | - rear_laser_sub_ = nh_.subscribe("/rear_laser/scan", 1, &MotionDecision::rear_laser_callback, this); |
27 | | - battery_voltage_sub_ = nh_.subscribe("/battery_voltage", 1, &MotionDecision::battery_voltage_callback, this); |
28 | | - footprint_sub_ = nh_.subscribe("/footprint", 1, &MotionDecision::footprint_callback, this); |
| 17 | + intersection_flag_pub_ = nh_.advertise<std_msgs::Bool>("intersection_flag", 1, true); |
| 18 | + cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1, true); |
| 19 | + |
| 20 | + emergency_stop_flag_sub_ = nh_.subscribe("emergency_stop", 1, &MotionDecision::emergency_stop_flag_callback, this); |
| 21 | + front_laser_sub_ = nh_.subscribe("front_laser/scan", 1, &MotionDecision::front_laser_callback, this); |
| 22 | + joy_sub_ = nh_.subscribe("joy", 1, &MotionDecision::joy_callback, this); |
| 23 | + local_path_cmd_vel_sub_ = nh_.subscribe("local_path/cmd_vel", 1, &MotionDecision::local_path_cmd_vel_callback, this); |
| 24 | + local_map_sub_ = nh_.subscribe("local_map", 1, &MotionDecision::local_map_callback, this); |
| 25 | + odom_sub_ = nh_.subscribe("odom", 1, &MotionDecision::odom_callback, this); |
| 26 | + rear_laser_sub_ = nh_.subscribe("rear_laser/scan", 1, &MotionDecision::rear_laser_callback, this); |
| 27 | + battery_voltage_sub_ = nh_.subscribe("battery_voltage", 1, &MotionDecision::battery_voltage_callback, this); |
| 28 | + footprint_sub_ = nh_.subscribe("footprint", 1, &MotionDecision::footprint_callback, this); |
29 | 29 |
|
30 | 30 | recovery_mode_flag_server_ = |
31 | | - nh_.advertiseService("/recovery/available", &MotionDecision::recovery_mode_flag_callback, this); |
32 | | - task_stop_flag_server_ = nh_.advertiseService("/task/stop", &MotionDecision::task_stop_flag_callback, this); |
| 31 | + nh_.advertiseService("recovery/available", &MotionDecision::recovery_mode_flag_callback, this); |
| 32 | + task_stop_flag_server_ = nh_.advertiseService("task/stop", &MotionDecision::task_stop_flag_callback, this); |
33 | 33 |
|
34 | 34 | load_params(); |
35 | 35 | if (params_.use_360_laser || params_.use_local_map) |
|
0 commit comments