diff --git a/README.md b/README.md index 843cb3d..975fc29 100644 --- a/README.md +++ b/README.md @@ -103,15 +103,15 @@ Joystick inputs: LB is used as a deadman switch and must be always pressed for t High-level Mode | Input buttons | Function -|-|- -1|None| Disabled -0|`LB`| Closed-loop velocity, open-loop steering +0|`LB`| Closed-loop velocity, open-loop steering (`LB` + `L3` is the same mode with a fixed 2 m/s ref) 1|`LB` + `RB`|Fully Open-loop 2|`LB` + `RT`|Closed-loop position, open-loop steering -3|`LB` + `A`|Closed-loop velocity, closed-loop steering +3|`LB` + `A`|Closed-loop velocity, closed-loop steering ( `LB` + `X` is the same mode with a fixed 2 m/s) 4|`LB` + `B`|Closed-loop position, closed-loop steering -5|`LB` + `X`|Closed-loop velocity, closed-loop steering +5|—---------| Empty template 6|`LB` + `Y`|Reset encoder command -7|`LB` + `LY`|Empty Template -8|`LB` + `Croos key Up/Down`| Empty Template +7|`LB` + `R3`| Empty Template +8|`LB` + `Cross key Up/Down`| Empty Template NaN|`LB` + `LT`|Joystick-based control disabled (no ctl_ref published) ## Low-level Controller Modes (Arduino modes) @@ -121,7 +121,7 @@ The low-level mode is the operating mode of the Arduino. The low-level mode is s Low-level Mode | Function -|- 0|Disabled -1|Open-loop PWM control +1|Open-loop voltage command (converted to PWM) 2|Closed-loop velocity (based on wheel-encoder feedback) -2|Closed-loop position (based on wheel-encoder feedback) +3|Closed-loop position (based on wheel-encoder feedback) 4|Reset encoder command diff --git a/racecar_arduino/Controller/platformio.ini b/racecar_arduino/Controller/platformio.ini index 6491f58..5647df4 100755 --- a/racecar_arduino/Controller/platformio.ini +++ b/racecar_arduino/Controller/platformio.ini @@ -12,6 +12,10 @@ platform = atmelavr board = megaatmega2560 framework = arduino +; Serial TX buffer of 256 bytes instead of 64: the whole 161-char sensor frame is queued at once and sent +; by the UART interrupt, so loop() does not wait (see sensorsCallback in src/main.cpp). Costs 192 B of RAM. +; Must be a build flag (not a #define in main.cpp) so that the Arduino core is compiled with it. +build_flags = -D SERIAL_TX_BUFFER_SIZE=256 lib_deps = nanopb/Nanopb@^0.4.91 diff --git a/racecar_arduino/Controller/src/PBUtils.cpp b/racecar_arduino/Controller/src/PBUtils.cpp index 5666354..eb85ab3 100755 --- a/racecar_arduino/Controller/src/PBUtils.cpp +++ b/racecar_arduino/Controller/src/PBUtils.cpp @@ -51,6 +51,62 @@ bool PBUtils::decodePb(char* inputString, int *subMsgId, int &nbsNewMsgs) return success; } +// pbSend version: 0 = original (String + sprintf), 1 = fast (same bytes on the wire) +#define PBSEND_FAST 1 +// PBSEND: #define PBSEND_FAST 1 // fast pbSend: same bytes, ~1.8 ms instead of ~9.7 ms (simavr, 16 MHz) + +#if PBSEND_FAST +/* + * Send protobufs messages to the serial port with format + * Fast version: same bytes as the String version below, but the frame is built in a fixed + * char array (2 hex digits per byte from a table) and sent with one Serial.write. + * No String (no malloc/realloc/free) and no sprintf. If an encoding fails, nothing is sent. + * + * @param nbs: The numbers of id to send + * @param ...: List of all the ids to send + */ +void PBUtils::pbSend(int nbs, ...) +{ + static const char hexDigits[] = "0123456789ABCDEF"; + char frame[2 * MAX_MSG_LEN + 16]; // "<" + id + "|" + 2 hex chars per byte + ";" + ">" (on the stack) + size_t len = 0; + frame[len++] = '<'; + + va_list idsToSend; + va_start(idsToSend, nbs); + for (int i = 0; i < nbs; ++i) + { + int id = va_arg(idsToSend, int); + uint8_t bufferOut[MAX_MSG_LEN]; + pb_ostream_t stream = pb_ostream_from_buffer(bufferOut, sizeof(bufferOut)); + char idText[12]; + itoa(id, idText, 10); // same text as String(id) + size_t idLen = strlen(idText); + + if (!pb_encode(&stream, idToType[id], idToMsg[id]) || + len + idLen + 2 * stream.bytes_written + 3 > sizeof(frame)) + { + va_end(idsToSend); + return; // encoding failed (or frame too long): nothing is sent, as before + } + + memcpy(frame + len, idText, idLen); + len += idLen; + frame[len++] = '|'; + for (size_t j = 0; j < stream.bytes_written; j++) + { + frame[len++] = hexDigits[bufferOut[j] >> 4]; // same as sprintf("%02X"): upper case, 2 digits + frame[len++] = hexDigits[bufferOut[j] & 0x0F]; + } + frame[len++] = ';'; + } + va_end(idsToSend); + + frame[len++] = '>'; + Serial.write((const uint8_t *)frame, len); // one call instead of Serial.print(String) +} + +#else // original pbSend /* * Send protobufs messages to the serial port with format * @@ -93,6 +149,7 @@ void PBUtils::pbSend(int nbs, ...) if (success) Serial.print(toSendBuilder); } +#endif // PBSEND_FAST /* * Convert an input string to a list of PB messages and ids diff --git a/racecar_arduino/Controller/src/main.cpp b/racecar_arduino/Controller/src/main.cpp index 68c9829..64657f1 100755 --- a/racecar_arduino/Controller/src/main.cpp +++ b/racecar_arduino/Controller/src/main.cpp @@ -40,14 +40,9 @@ const int str_pin = 9; // Servo const int dri_pwm_pin = 6; // H bridge drive pwm const int dri_dir_pin = 42; // -// debug -long timer_debug = 0; -long time_micros = 0; -long time_micros_last = 0; - // Prototype void cmdCallback(); -void sensorsCallback(unsigned long dt); +void sensorsCallback(unsigned long dt_com_us); /////////////////////////////////////////////////////////////////// // Parameters @@ -65,10 +60,10 @@ const float pos_kd = 0.0; const float pos_ki = 0.0; const float pos_ei_sat = 10000.0; -// Loop period -const unsigned long time_period_low = 2; // 500 Hz for internal PID loop -const unsigned long time_period_high = 25; // 50 Hz for ROS communication -const unsigned long time_period_com = 1000; // 1000 ms = max com delay (watchdog) +// Comparison targets [us] +const unsigned long period_ctl_us = 2000; // 500 Hz internal PID loop +const unsigned long period_com_us = 20000; // 50 Hz ROS communication +const unsigned long period_watchdog_us = 1000000; // 1 s max without a command // Hardware min-zero-max range for the steering servo and the drive const int min_str_angle = 45; @@ -78,7 +73,7 @@ const int pwm_min_dri = -511; const int pwm_zer_dri = 0; const int pwm_max_dri = 511; -const int dri_wakeup_time = 20; // micro second +const int dri_wakeup_us = 20; // [us] fixed H-bridge wake pulse // Units Conversion const double batteryV = 8; @@ -115,13 +110,19 @@ float vel_error_int = 0; float pos_error_int = 0; // Loop timing -unsigned long time_now = 0; -unsigned long time_last_low = 0; -unsigned long time_last_high = 0; -unsigned long time_last_com = 0; // com watchdog +// time_ = recorded micros() value, period_ = comparison target, dt_ = measured delta +unsigned long time_now_us = 0; +unsigned long time_last_ctl_us = 0; +unsigned long time_last_com_us = 0; +unsigned long time_last_cmd_us = 0; // watchdog: last received command +unsigned long dt_ctl_us = 0; +unsigned long dt_com_us = 0; +unsigned long dt_pause_us = 0; // duration of the last sensorsCallback +float dt_ctl_ms = 0 ; +float dt_com_ms = 0 ; // For odometry -signed long enc_last_high = 0; +signed long enc_last_com = 0; /////////////////////////////////////////////////////////////////// // Encoder init/read/reset functions @@ -259,7 +260,7 @@ void set_pwm(int pwm) if (dri_standby == 1) { digitalWrite(dri_pwm_pin, HIGH); - delayMicroseconds(dri_wakeup_time); + delayMicroseconds(dri_wakeup_us); dri_standby = 0; } @@ -317,7 +318,7 @@ const unsigned long baud_rate = 115200; /////////////////////////////////////////////////////////////////// // Controller One tick /////////////////////////////////////////////////////////////////// -void ctl(int dt_low) +void ctl(float dt_ctl_ms) // [ms] measured control delta since the last tick { /////////////////////////////////////////////// // STEERING CONTROL @@ -340,7 +341,7 @@ void ctl(int dt_low) // Velocity computation // TODO: VOUS DEVEZ COMPLETEZ LA DERIVEE FILTRE ICI - float vel_raw = (enc_now - enc_old) * tick2m / dt_low * 1000; + float vel_raw = (enc_now - enc_old) * tick2m / dt_ctl_ms * 1000.0f; float alpha = 0; // TODO float vel_fil = vel_raw; // Filter TODO @@ -414,6 +415,8 @@ void ctl(int dt_low) // Reset encoder counts clearEncoderCount(); + enc_now = readEncoder(); // counter is 0 now: no false speed step next tick + enc_last_com = enc_now; // same for data[9] (distance since last publish) // reset integral actions vel_error_int = 0; @@ -476,13 +479,13 @@ void setup() void loop() { - time_now = millis(); + time_now_us = micros(); // [us] 4 us resolution; wraps after 71.6 min (unsigned differences stay correct) ///////////////////////////////////////////////////////////// // Watchdog: stop the car if no recent communication from ROS ////////////////////////////////////////////////////////////// - if ((time_now - time_last_com) > time_period_com) + if ((time_now_us - time_last_cmd_us) > period_watchdog_us) { // All-stop dri_ref = 0; // velocity set-point @@ -493,15 +496,22 @@ void loop() // Low-level controller /////////////////////////////////////// - if ((time_now - time_last_low) > time_period_low) + dt_ctl_us = time_now_us - time_last_ctl_us; + + if (dt_ctl_us > period_ctl_us) // 500 Hz control tick { - ctl(time_now - time_last_low); // one control tick - timer_debug = time_micros - time_micros_last; /// + time_last_ctl_us = time_now_us; + + dt_ctl_ms = dt_ctl_us * 0.001f; - time_last_low = time_now; - time_micros_last = time_micros; + // one control tick, dt in [ms] + ctl(dt_ctl_ms); } + //////////////////////////////////////// + // Receive commands from ROS + /////////////////////////////////////// + if (inCmdComplete) { inCmdComplete = false; @@ -527,12 +537,20 @@ void loop() } } - unsigned long dt = time_now - time_last_high; - if (dt > time_period_high) + dt_com_us = time_now_us - time_last_com_us; + + //////////////////////////////////////// + // Publish sensors data + /////////////////////////////////////// + + if (dt_com_us > period_com_us) // 50 Hz communication tick { - sensorsCallback(dt); - time_last_high = time_now; - enc_last_high = enc_now; + unsigned long time_pause_us = micros(); // clock at the start of sensorsCallback + sensorsCallback(dt_com_us); + dt_pause_us = micros() - time_pause_us; + time_last_com_us = time_now_us; + enc_last_com = enc_now; + dt_com_ms = dt_com_us * 0.001f; } } @@ -544,26 +562,27 @@ void cmdCallback() dri_ref = cmdMsg.data[1]; // volt or m/s or m ctl_mode = cmdMsg.data[2]; // 1 or 2 or 3*/ - time_last_com = millis(); + time_last_cmd_us = micros(); } -void sensorsCallback(unsigned long dt) +void sensorsCallback(unsigned long dt_com_us) { - sensorsMsg.data_count = 19; + sensorsMsg.data_count = 19; // do not change: pb2roscpp and arduino_sensors expect exactly 19 floats sensorsMsg.data[0] = pos_now; // wheel position in m sensorsMsg.data[1] = vel_old; // wheel velocity in m/sec // For DEBUG sensorsMsg.data[2] = (float)dri_ref; // set point received by arduino sensorsMsg.data[3] = (float)dri_cmd; // drive set point in volts - // sensorsMsg.data[3] = (float)Serial.available(); // futile: we SHOULD NOT receive anything if Serial is not available. - sensorsMsg.data[4] = (float)dri_pwm; // drive set point in pwm - sensorsMsg.data[5] = (float)enc_now; // raw encoder counts + sensorsMsg.data[4] = (float)dt_ctl_ms; + // sensorsMsg.data[4] = (float)dri_pwm; // drive set point in pwm + // sensorsMsg.data[5] = (float)enc_now; // raw encoder counts + sensorsMsg.data[5] = (float)dt_pause_us; sensorsMsg.data[6] = (float)str_ref; // steering angle (don't remove/change, used for GRO830) sensorsMsg.data[7] = (float)(ctl_mode); // for com debug - sensorsMsg.data[8] = (float)dt; // time elapsed since last publish (don't remove/change, used for GRO830) + sensorsMsg.data[8] = (float)dt_com_us * 0.001f; // [ms] time elapsed since last publish (don't remove/change, used for GRO830) sensorsMsg.data[9] = - (enc_now - enc_last_high) * tick2m; // distance travelled since last publish (don't remove/change, used for GRO830) + (enc_now - enc_last_com) * tick2m; // distance travelled since last publish (don't remove/change, used for GRO830) // Read IMU (don't remove/change, used for GRO830) #ifdef IMU @@ -580,7 +599,6 @@ void sensorsCallback(unsigned long dt) #endif pbUtils.pbSend(1, SENSORS); - Serial.flush(); } // ======================================== SERIAL ======================================== diff --git a/racecar_autopilot/racecar_autopilot/rosbag2csv.py b/racecar_autopilot/racecar_autopilot/rosbag2csv.py index 802cc62..e9fec37 100644 --- a/racecar_autopilot/racecar_autopilot/rosbag2csv.py +++ b/racecar_autopilot/racecar_autopilot/rosbag2csv.py @@ -34,7 +34,7 @@ def get_rosbag_options(path, serialization_format='cdr'): - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='mcap') + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='') # auto-detect mcap/sqlite3 converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, @@ -97,7 +97,7 @@ def dump_bag(bag_path): if hasattr(msg, "header"): t = msg.header.stamp.sec + 1e-9*msg.header.stamp.nanosec else: - t = ts + t = ts * 1e-9 # bag time [ns] -> [s], same unit as header stamps if start_time is None: start_time = t print(','.join([str(t - start_time)] + diff --git a/racecar_autopilot/racecar_autopilot/slash_controller.py b/racecar_autopilot/racecar_autopilot/slash_controller.py index 66254f6..9cb5607 100755 --- a/racecar_autopilot/racecar_autopilot/slash_controller.py +++ b/racecar_autopilot/racecar_autopilot/slash_controller.py @@ -28,14 +28,21 @@ def __init__(self): self.dt = 0.05 self.timer = self.create_timer(self.dt, self.timed_controller) - # Paramters + # Parameters # Controller self.steering_offset = 0.0 # To adjust according to the vehicle - self.K_autopilot = None # TODO: DESIGN LQR - - self.K_parking = None # TODO: DESIGN PLACEMENT DE POLES + # TODO: test on real racecar to validate + # Design offline, paste here. Shapes must match the x you assemble below. + self.params_autopilot = { + "K": None, # (m, n) + "ubar": None, # (m,) feedforward + } + self.params_parking = { + "K": None, + "ubar": None, + } # Memory @@ -96,53 +103,76 @@ def timed_controller(self): self.arduino_mode = 3 self.steering_cmd = self.steering_ref + self.steering_offset - # APP4 (closed-loop steering) controllers bellow - elif self.high_level_mode == 3 or self.high_level_mode == 5: - # Closed-loop velocity and steering + # ---------------------------------------------------------- + # TODO: test on real racecar to validate + # APP4 — catalog of signals available + # + # 1. Measurements (last value received) + # self.position encoder, longitudinal position [m] + # self.velocity encoder, longitudinal speed [m/s] + # self.laser_y lidar, lateral position [m] + # self.laser_theta lidar, heading [rad] + # self.laser_dy_fill filtered dy/dt [m/s] + # + # 2. References (from ctl_ref) + # self.propulsion_ref longitudinal ref (speed or position, + # depends on the high-level joystick mode) + # self.steering_ref steering / lateral ref + # + # 3. Controllable actions — two channels; meaning depends on + # arduino_mode + # u[0] propulsion + # arduino_mode == 1 → open-loop voltage V [V] + # arduino_mode == 2 → closed-loop velocity setpoint + # arduino_mode == 3 → closed-loop position setpoint + # u[1] steering δ (steering_offset is added after the policy) + # self.arduino_mode is a choice you must set below (1 / 2 / 3) + # ---------------------------------------------------------- + + elif self.high_level_mode == 3: + # Autopilot (high-level mode 3) ######################################################### - # TODO: COMPLETEZ LE CONTROLLER - - # Auto-pilot # 1 + # TODO: complètez — assemblage de x, r et arduino_mode - # x = [ ?,? ,.... ] - # r = [ ?,? ,.... ] - # u = [ servo_cmd , prop_cmd ] + # x = np.array([ ... ]) # pick from the catalog + # r = np.array([ ... ]) x = None r = None - u = self.controller1(x, r) + u = self.ctl_autopilot(x, r) - self.steering_cmd = u[1] + self.steering_offset self.propulsion_cmd = u[0] - self.arduino_mode = 0 # Mode ??? on arduino - # TODO: COMPLETEZ LE CONTROLLER + self.steering_cmd = u[1] + self.steering_offset + self.arduino_mode = 0 # complètez: 1, 2 ou 3 (0 = sortie nulle) ######################################################### elif self.high_level_mode == 4: - # Closed-loop position and steering + # Parking (high-level mode 4) ######################################################### - # TODO: COMPLETEZ LE CONTROLLER - - # Auto-pilot # 1 + # TODO: complètez — assemblage de x, r et arduino_mode - # x = [ ?,? ,.... ] - # r = [ ?,? ,.... ] - # u = [ servo_cmd , prop_cmd ] + # x = np.array([ ... ]) # pick from the catalog + # r = np.array([ ... ]) x = None r = None - u = self.controller2(x, r) + u = self.ctl_parking(x, r) - self.steering_cmd = u[1] + self.steering_offset self.propulsion_cmd = u[0] - self.arduino_mode = 0 # Mode ??? on arduino - # TODO: COMPLETEZ LE CONTROLLER + self.steering_cmd = u[1] + self.steering_offset + self.arduino_mode = 0 # complètez: 1, 2 ou 3 (0 = sortie nulle) ######################################################### + elif self.high_level_mode == 5: + # Template for custom controllers + self.steering_cmd = 0 + self.steering_offset + self.propulsion_cmd = 0 + self.arduino_mode = 0 # Mode ??? on arduino + elif self.high_level_mode == 6: # Reset encoders self.propulsion_cmd = 0 @@ -165,25 +195,27 @@ def timed_controller(self): self.send_arduino() ####################################### - def controller1(self, y, r): + def ctl_autopilot(self, x, r, t=0, params=None): - # Control Law TODO + params = self.params_autopilot if params is None else params - u = np.array([0, 0]) # placeholder - - # u = self.K_autopilot @ (r - x) + # K = params["K"] + # ubar = params["ubar"] + # u = ubar - K @ (x - r) + u = np.zeros(2) return u ####################################### - def controller2(self, y, r): - - # Control Law TODO + def ctl_parking(self, x, r, t=0, params=None): - u = np.array([0, 0]) # placeholder + params = self.params_parking if params is None else params - # u = self.K_parking @ (r - x) + # K = params["K"] + # ubar = params["ubar"] + # u = ubar - K @ (x - r) + u = np.zeros(2) return u ####################################### @@ -224,23 +256,6 @@ def send_arduino(self): # Publish cmd msg self.pub_cmd.publish(cmd_prop) - ####################################### - def pub_kinematic(self): - # init encd_info msg - pos = Twist() - vel = Twist() - acc = Twist() - - # Msg - pos.linear.x = 0 - vel.linear.x = 0 - acc.linear.x = 0 - - # Publish cmd msg - self.pub_pos.publish(pos) - self.pub_vel.publish(vel) - self.pub_acc.publish(acc) - def main(args=None): rclpy.init(args=args) diff --git a/racecar_autopilot/racecar_autopilot/wall_estimator.py b/racecar_autopilot/racecar_autopilot/wall_estimator.py index f95e7e7..8824f00 100644 --- a/racecar_autopilot/racecar_autopilot/wall_estimator.py +++ b/racecar_autopilot/racecar_autopilot/wall_estimator.py @@ -158,7 +158,7 @@ def estimate_car_position(self, scan_msg): if n_good_scan > 2: - # Estimate left wall line from points + # Estimate right wall line from points d = np.array(d_data) theta = np.array(theta_data) self.theta_right, self.y_right = self.estimate_line_from_points(d, theta) diff --git a/racecar_teleop/racecar_teleop/slash_teleop.py b/racecar_teleop/racecar_teleop/slash_teleop.py index 08a6355..d074d94 100755 --- a/racecar_teleop/racecar_teleop/slash_teleop.py +++ b/racecar_teleop/racecar_teleop/slash_teleop.py @@ -11,119 +11,122 @@ class Teleop(Node): """ teleoperation """ - def __init__(self): - super().__init__('Teleop') + def __init__(self): + super().__init__("Teleop") - self.max_vel = self.declare_parameter('max_vel', 4.0).value - self.max_volt = self.declare_parameter('max_volt', 8.0).value - self.maxStAng = self.declare_parameter('max_angle', 40).value - self.ps4 = self.declare_parameter('ps4', False).value + self.max_vel = self.declare_parameter("max_vel", 4.0).value + self.max_volt = self.declare_parameter("max_volt", 8.0).value + self.maxStAng = self.declare_parameter("max_angle", 40).value + self.ps4 = self.declare_parameter("ps4", False).value - self.cmd2rad = self.maxStAng*2*3.1416/360 + self.cmd2rad = self.maxStAng * 2 * 3.1416 / 360 self.joystickCompatibilityWarned = False + self.pub_cmd = self.create_publisher(Twist, "ctl_ref", 1) - self.pub_cmd = self.create_publisher(Twist, 'ctl_ref', 1) - # Always create subscribers last - self.sub_joy = self.create_subscription(Joy, 'joy', self.joy_callback, 1) + self.sub_joy = self.create_subscription(Joy, "joy", self.joy_callback, 1) - + ####################################### - ####################################### - - def joy_callback( self, joy_msg ): + def joy_callback(self, joy_msg): """ """ + # TODO: vérifier le mode de la manette (D ou X) avec « ros2 topic echo /joy ». + # Les indices ci-dessous supposent le mode D (DirectInput), comme le README. min_axes = 5 if self.ps4 else 4 if len(joy_msg.axes) < min_axes or len(joy_msg.buttons) < 7: if not self.joystickCompatibilityWarned: - self.get_logger().info("slash_teleop: Received topic doesn't have enough axes and/or buttons. If a Logitech gamepad is used, make sure also it is in X mode. Will not warn again.") + self.get_logger().info( + "slash_teleop: Received topic doesn't have enough axes and/or buttons. If a Logitech gamepad is used, make sure also it is in X mode. Will not warn again." + ) self.joystickCompatibilityWarned = True return - self.joystickCompatibilityWarned = False # reset in case we switch mode on the gamepad + self.joystickCompatibilityWarned = ( + False # reset in case we switch mode on the gamepad + ) + + propulsion_user_input = joy_msg.axes[3] # Up-down Right joystick + steering_user_input = joy_msg.axes[0] # Left-right left joystick + + self.cmd_msg = Twist() - propulsion_user_input = joy_msg.axes[3] # Up-down Right joystick - steering_user_input = joy_msg.axes[0] # Left-right left joystick - - self.cmd_msg = Twist() - # Software deadman switch - #If left button is active - if (joy_msg.buttons[4]): - - #No button pressed (see below) + # If left button is active + if joy_msg.buttons[4]: + + # No button pressed (see below) # Closed-loop velocity, Open-loop steering, control mode = 0 - - #If right button is active - if (joy_msg.buttons[5]): + + # If right button is active + if joy_msg.buttons[5]: # Fully Open-Loop - self.cmd_msg.linear.x = propulsion_user_input * self.max_volt #[volts] + self.cmd_msg.linear.x = propulsion_user_input * self.max_volt # [volts] self.cmd_msg.angular.z = steering_user_input * self.cmd2rad - self.cmd_msg.linear.z = 1.0 #CtrlChoice - - elif (joy_msg.buttons[10]): # RJP + self.cmd_msg.linear.z = 1.0 # CtrlChoice + + elif joy_msg.buttons[10]: # L3 (mode D) """ GRO501-1: closed-loop velocity fixed @ X m/s, open-loop steering, where X is determined "on-site". """ - self.cmd_msg.linear.x = 2.0 # m/s + self.cmd_msg.linear.x = 2.0 # m/s self.cmd_msg.angular.z = steering_user_input * self.cmd2rad - self.cmd_msg.linear.z = 0.0 # high-level mode - - #If right trigger is active - elif (joy_msg.buttons[7]): # START + self.cmd_msg.linear.z = 0.0 # high-level mode + + # If right trigger is active + elif joy_msg.buttons[7]: # RT (mode D) """ GRO501-1: closed-loop position fixed @ X m, open-loop steering, where X is determined "on-site". """ - self.cmd_msg.linear.x = 2.0 # [m] + self.cmd_msg.linear.x = 2.0 # [m] self.cmd_msg.angular.z = steering_user_input * self.cmd2rad - self.cmd_msg.linear.z = 2.0 #CtrlChoice - - #If button A is active - elif(joy_msg.buttons[1]): - # Closed-loop velocity, Closed-loop steering - self.cmd_msg.linear.x = propulsion_user_input * self.max_vel #[m/s] - self.cmd_msg.angular.z = steering_user_input # [m] - self.cmd_msg.linear.z = 3.0 # Control mode - - #If button B is active - elif(joy_msg.buttons[2]): - # Closed-loop position, Closed-loop steering - self.cmd_msg.linear.x = propulsion_user_input # [m] - self.cmd_msg.angular.z = steering_user_input # [m] - self.cmd_msg.linear.z = 4.0 # Control mode - - #If button x is active - elif(joy_msg.buttons[0]): - # Closed-loop velocity with fixed 1 m/s ref, Closed-loop steering - self.cmd_msg.linear.x = 2.0 #[m/s] - self.cmd_msg.angular.z = 0.0 # [m] - self.cmd_msg.linear.z = 5.0 # Control mode - - #If button y is active - elif(joy_msg.buttons[3]): + self.cmd_msg.linear.z = 2.0 # CtrlChoice + + # If button A is active + elif joy_msg.buttons[1]: + # Closed-loop velocity, Closed-loop steering + self.cmd_msg.linear.x = propulsion_user_input * self.max_vel # [m/s] + self.cmd_msg.angular.z = steering_user_input # [m] + self.cmd_msg.linear.z = 3.0 # Control mode + + # If button B is active + elif joy_msg.buttons[2]: + # Closed-loop position, Closed-loop steering + self.cmd_msg.linear.x = propulsion_user_input # [m] + self.cmd_msg.angular.z = steering_user_input # [m] + self.cmd_msg.linear.z = 4.0 # Control mode + + # If button x is active + elif joy_msg.buttons[0]: + # Autopilot at a fixed 2 m/s, zero lateral ref (same high-level mode as A) + self.cmd_msg.linear.x = 2.0 # [m/s] + self.cmd_msg.angular.z = 0.0 # [m] + self.cmd_msg.linear.z = 3.0 # Control mode + + # If button y is active + elif joy_msg.buttons[3]: # Reset Encoder - self.cmd_msg.linear.x = 0.0 + self.cmd_msg.linear.x = 0.0 self.cmd_msg.angular.z = 0.0 - self.cmd_msg.linear.z = 6.0 # Control mode - - #If left trigger is active - elif (joy_msg.buttons[6]): + self.cmd_msg.linear.z = 6.0 # Control mode + + # If left trigger is active + elif joy_msg.buttons[6]: # No ctl_ref msg published! return - - #If right joy pushed + + # If right joy pushed # elif(joy_msg.buttons[11]): # # Template for a custom mode # self.cmd_msg.linear.x = 0.0 # self.cmd_msg.angular.z = 0.0 # self.cmd_msg.linear.z = 7.0 # Control mode - - #If bottom arrow is active + + # If bottom arrow is active # elif(joy_msg.axes[7]): # # Template for a custom mode # self.cmd_msg.linear.x = 0.0 @@ -134,24 +137,23 @@ def joy_callback( self, joy_msg ): # No active button else: # Closed-loop velocity, Open-loop steering - self.cmd_msg.linear.x = propulsion_user_input * self.max_vel #[m/s] + self.cmd_msg.linear.x = propulsion_user_input * self.max_vel # [m/s] self.cmd_msg.angular.z = steering_user_input * self.cmd2rad - self.cmd_msg.linear.z = 0.0 # Control mode - + self.cmd_msg.linear.z = 0.0 # Control mode + # Deadman is un-pressed else: # All-stop - self.cmd_msg.linear.x = 0.0 + self.cmd_msg.linear.x = 0.0 self.cmd_msg.linear.y = 0.0 - self.cmd_msg.linear.z = -1.0 + self.cmd_msg.linear.z = -1.0 self.cmd_msg.angular.x = 0.0 self.cmd_msg.angular.y = 0.0 - self.cmd_msg.angular.z = 0.0 - + self.cmd_msg.angular.z = 0.0 # Publish cmd msg - self.pub_cmd.publish( self.cmd_msg ) - + self.pub_cmd.publish(self.cmd_msg) + def main(args=None): rclpy.init(args=args) @@ -159,6 +161,7 @@ def main(args=None): rclpy.spin(node) rclpy.shutdown() + ############## -if __name__ == '__main__': +if __name__ == "__main__": main()