From fb64fd7efda1b3174e265f62f945b0b9dde0e81b Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 7 Apr 2026 00:56:35 -0400 Subject: [PATCH 1/3] Post SAR updates --- src/bringup/CMakeLists.txt | 3 +- src/bringup/scripts/cmd_vel_to_rmd.py | 83 ++++++ src/config/cyclonedds/config_base.xml | 23 ++ src/config/cyclonedds/config_jetson.xml | 4 +- .../arm/arm.rmd.ros2_control.xacro | 4 +- .../arm/arm.smc.ros2_control.xacro | 2 +- .../drive/drive.odrive.ros2_control.xacro | 4 +- .../drive/drive.rmd.ros2_control.xacro | 10 +- .../src/odrive_hardware_interface.cpp | 4 +- .../umdloop_can_library/SocketCanBus.hpp | 12 +- .../umdloop_can_library/src/SocketCanBus.cpp | 119 +++++---- .../config/athena_drive_controllers.yaml | 3 +- .../drive_bringup/config/teleop_twist.yaml | 2 +- .../drive/drive_controllers/CMakeLists.txt | 3 +- .../config/rear_ackermann_controller.yaml | 29 +-- .../rear_ackermann_controller.hpp | 17 +- .../src/rear_ackermann_controller.cpp | 243 +++++++++--------- .../athena_planner/config/nav2_params.yaml | 139 +++++++--- src/tools/scripts/add_internet.sh | 3 + src/tools/scripts/can_test.sh | 160 ++++++++++++ src/tools/scripts/jetson_canable.sh | 7 + src/tools/scripts/nav2_lifecycle_status.sh | 0 src/tools/scripts/send_gps_goal.sh | 43 ---- 23 files changed, 600 insertions(+), 317 deletions(-) create mode 100755 src/bringup/scripts/cmd_vel_to_rmd.py create mode 100644 src/config/cyclonedds/config_base.xml create mode 100755 src/tools/scripts/add_internet.sh create mode 100755 src/tools/scripts/can_test.sh create mode 100755 src/tools/scripts/jetson_canable.sh mode change 100755 => 100644 src/tools/scripts/nav2_lifecycle_status.sh mode change 100755 => 100644 src/tools/scripts/send_gps_goal.sh diff --git a/src/bringup/CMakeLists.txt b/src/bringup/CMakeLists.txt index cb97bf0d..c1da79f3 100644 --- a/src/bringup/CMakeLists.txt +++ b/src/bringup/CMakeLists.txt @@ -17,9 +17,10 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/config) endif() # Install controller_switcher script from bringup package if present -if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/controller_switcher.py) +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/controller_switcher.py OR ${CMAKE_CURRENT_SOURCE_DIR}/scripts/cmd_vel_to_rmd.py) install(PROGRAMS scripts/controller_switcher.py + scripts/cmd_vel_to_rmd.py DESTINATION lib/${PROJECT_NAME} ) endif() diff --git a/src/bringup/scripts/cmd_vel_to_rmd.py b/src/bringup/scripts/cmd_vel_to_rmd.py new file mode 100755 index 00000000..09944a48 --- /dev/null +++ b/src/bringup/scripts/cmd_vel_to_rmd.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +import socket +import struct +import time + +CAN_FMT = "=IH2x8s" +CAN_SIZE = struct.calcsize(CAN_FMT) + + +CAN_IDS = [0x145, 0x141, 0x142, 0x144] +CAN_INTERFACE = "can1" +MAX_SPEED_DPS = 500.0 +SCALE = 100.0 +TOPIC = "/rear_ackermann_controller/reference" + + +SPEED_CONTROL_CMD = 0xA2 + + + + +class CmdVelToRMD(Node): + def __init__(self): + super().__init__("cmd_vel_to_rmd") + + + self._sock = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW) + self._sock.bind((CAN_INTERFACE,)) + self.get_logger().info(f"Listening on {TOPIC}, sending to {[hex(i) for i in CAN_IDS]}") + + + self.create_subscription(TwistStamped, TOPIC, self._cb, 10) + + + def _cb(self, msg: TwistStamped): + speed_dps = msg.twist.linear.x * SCALE + speed_dps = max(-MAX_SPEED_DPS, min(MAX_SPEED_DPS, speed_dps)) + speed_ctrl = int(speed_dps / 0.01) & 0xFFFFFFFF + + + data = bytes([ + SPEED_CONTROL_CMD, 0x00, 0x00, 0x00, + (speed_ctrl) & 0xFF, + (speed_ctrl >> 8) & 0xFF, + (speed_ctrl >> 16) & 0xFF, + (speed_ctrl >> 24) & 0xFF, + ]) + + + for can_id in CAN_IDS: + self._sock.send(struct.pack(CAN_FMT, can_id, 8, data)) + time.sleep(.001) + + + def destroy_node(self): + self._sock.close() + super().destroy_node() + + + + +def main(args=None): + rclpy.init(args=args) + node = CmdVelToRMD() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + + + +if __name__ == "__main__": + main() + + + diff --git a/src/config/cyclonedds/config_base.xml b/src/config/cyclonedds/config_base.xml new file mode 100644 index 00000000..7d6f6d0d --- /dev/null +++ b/src/config/cyclonedds/config_base.xml @@ -0,0 +1,23 @@ + + + + + + + + false + + + + + + + auto + 1000 + + + + + + diff --git a/src/config/cyclonedds/config_jetson.xml b/src/config/cyclonedds/config_jetson.xml index c50cbd6d..8ed3b325 100644 --- a/src/config/cyclonedds/config_jetson.xml +++ b/src/config/cyclonedds/config_jetson.xml @@ -10,8 +10,8 @@ https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclo - - + + auto 1000 diff --git a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro index a8bfa77c..7efeaf8e 100644 --- a/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.rmd.ros2_control.xacro @@ -4,14 +4,14 @@ rmd_ros2_control/RMDHardwareInterface - 2 + 10 5 0 can0 - 0x14A + 0x149 100 1.0 150 diff --git a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro index e17001a4..5a1aeb5c 100644 --- a/src/description/ros2_control/arm/arm.smc.ros2_control.xacro +++ b/src/description/ros2_control/arm/arm.smc.ros2_control.xacro @@ -4,7 +4,7 @@ smc_ros2_control/SMCHardwareInterface - 2 + 10 5 0 can0 diff --git a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro index b9dfb868..e584d59e 100644 --- a/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.odrive.ros2_control.xacro @@ -10,7 +10,7 @@ - 5 + 7 26 @@ -21,7 +21,7 @@ - 6 + 5 26 diff --git a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro index be3fdc2d..fa3f6f01 100644 --- a/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro +++ b/src/description/ros2_control/drive/drive.rmd.ros2_control.xacro @@ -4,7 +4,7 @@ rmd_ros2_control/RMDHardwareInterface - 30 + 10 5 0 can0 @@ -24,7 +24,7 @@ - 0x149 + 0x145 1 1.0 150 @@ -37,7 +37,7 @@ - 0x141 + 0x144 1 -1.0 150 @@ -50,7 +50,7 @@ - 0x143 + 0x141 1 1.0 150 @@ -63,4 +63,4 @@ - \ No newline at end of file + diff --git a/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp b/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp index 96e29533..b7bd4cf0 100644 --- a/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp +++ b/src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp @@ -283,14 +283,14 @@ return_type ODriveHardwareInterface::write(const rclcpp::Time&, const rclcpp::Du msg.Input_Pos = (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI); msg.Vel_FF = axis.vel_input_enabled_ ? ((axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)) : 0.0f; msg.Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_) : 0.0f; - // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing positions for ODrive %d Setpoint: %f, Joint angle of motor (rev): %f", axis.node_id_, axis.pos_setpoint_, (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); + //RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing positions for ODrive %d Setpoint: %f, Joint angle of motor (rev): %f", axis.node_id_, axis.pos_setpoint_, (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); axis.send(msg); } else if (axis.vel_input_enabled_) { Set_Input_Vel_msg_t msg; msg.Input_Vel = (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI); msg.Input_Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_): 0.0f; - // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing velocities for ODrive %d Joint velocity of motor (rev/s): %f", axis.node_id_, (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); + RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing velocities for ODrive %d Joint velocity of motor (rev/s): %f", axis.node_id_, (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)); axis.send(msg); } else if (axis.torque_input_enabled_) { diff --git a/src/infrastructure/umdloop_can_library/include/umdloop_can_library/SocketCanBus.hpp b/src/infrastructure/umdloop_can_library/include/umdloop_can_library/SocketCanBus.hpp index 3b61ec00..09066685 100644 --- a/src/infrastructure/umdloop_can_library/include/umdloop_can_library/SocketCanBus.hpp +++ b/src/infrastructure/umdloop_can_library/include/umdloop_can_library/SocketCanBus.hpp @@ -1,9 +1,11 @@ #pragma once #include "ICanBus.hpp" -#include #include #include +#include +#include +#include namespace CANLib { @@ -17,13 +19,21 @@ class SocketCanBus : public ICanBus { bool send(const CanFrame& frame) override; private: + static constexpr size_t kMaxTxQueueDepth = 256; + int socketFd_; ReceiveCallback receiveCallback_; std::thread receiveThread_; + std::thread txThread_; std::atomic running_; std::string interfaceName_; + std::queue txQueue_; + std::mutex txMutex_; + std::condition_variable txCv_; + void receiveLoop_(); + void txLoop_(); }; } // namespace CANLib \ No newline at end of file diff --git a/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp b/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp index a15d7f64..0b153200 100644 --- a/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp +++ b/src/infrastructure/umdloop_can_library/src/SocketCanBus.cpp @@ -8,6 +8,7 @@ #include #include #include +#include namespace CANLib { @@ -16,14 +17,14 @@ namespace { std::memset(&can_frame, 0, sizeof(can_frame)); can_frame.can_id = frame.id; can_frame.can_dlc = frame.dlc; - + if (frame.is_extended) { can_frame.can_id |= CAN_EFF_FLAG; } if (frame.is_rtr) { can_frame.can_id |= CAN_RTR_FLAG; } - + std::copy(frame.data.begin(), frame.data.begin() + frame.dlc, can_frame.data); } @@ -58,9 +59,10 @@ bool SocketCanBus::open(const std::string& interface_name, ReceiveCallback callb // Get interface index struct ifreq ifr; - std::strcpy(ifr.ifr_name, interface_name.c_str()); + std::strncpy(ifr.ifr_name, interface_name.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; if (ioctl(socketFd_, SIOCGIFINDEX, &ifr) < 0) { - std::cerr << "Failed to get interface index for " << interface_name + std::cerr << "Failed to get interface index for " << interface_name << ": " << strerror(errno) << std::endl; ::close(socketFd_); socketFd_ = -1; @@ -74,31 +76,38 @@ bool SocketCanBus::open(const std::string& interface_name, ReceiveCallback callb addr.can_ifindex = ifr.ifr_ifindex; if (bind(socketFd_, (struct sockaddr*)&addr, sizeof(addr)) < 0) { - std::cerr << "Failed to bind socket to " << interface_name + std::cerr << "Failed to bind socket to " << interface_name << ": " << strerror(errno) << std::endl; ::close(socketFd_); socketFd_ = -1; return false; } - // Enable receipt of own messages (loopback) - int loopback = 1; - setsockopt(socketFd_, SOL_CAN_RAW, CAN_RAW_LOOPBACK, - &loopback, sizeof(loopback)); + // Disable loopback (do not receive our own transmitted frames) + int loopback = 0; + setsockopt(socketFd_, SOL_CAN_RAW, CAN_RAW_LOOPBACK, &loopback, sizeof(loopback)); - // Enable receipt of CAN FD frames - int canfd_on = 1; - setsockopt(socketFd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, - &canfd_on, sizeof(canfd_on)); + // Disable CAN FD — we only use classical CAN frames + int canfd_off = 0; + setsockopt(socketFd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &canfd_off, sizeof(canfd_off)); + + // Increase the socket send buffer to 1 MB (kernel doubles this, giving ~2 MB effective) + int sndbuf = 1048576; + setsockopt(socketFd_, SOL_SOCKET, SO_SNDBUF, &sndbuf, sizeof(sndbuf)); running_ = true; receiveThread_ = std::thread(&SocketCanBus::receiveLoop_, this); + txThread_ = std::thread(&SocketCanBus::txLoop_, this); return true; } void SocketCanBus::close() { if (running_) { running_ = false; + txCv_.notify_one(); + if (txThread_.joinable()) { + txThread_.join(); + } if (receiveThread_.joinable()) { receiveThread_.join(); } @@ -110,54 +119,65 @@ void SocketCanBus::close() { } bool SocketCanBus::send(const CanFrame& frame) { - if (!running_ || socketFd_ < 0) return false; - - ::can_frame can_frame; - mapCanFrameToSocketCan(frame, can_frame); - - ssize_t nbytes = write(socketFd_, &can_frame, sizeof(can_frame)); - - //static int send_count = 0; - //static auto last_time = std::chrono::steady_clock::now(); - //send_count++; - //if (send_count % 100 == 0) { - // auto now = std::chrono::steady_clock::now(); - // auto duration = std::chrono::duration_cast(now - last_time).count(); - // std::cout << "CAN: messages sent: " << send_count << " | Time for last 100: " << duration << " ms" << std::endl; - // last_time = now; - //} - - if (nbytes != sizeof(can_frame)) { - std::cerr << "CAN: Failed to send frame - ID: 0x" << std::hex << frame.id - << std::dec << ", DLC: " << static_cast(frame.dlc) - << ", Data: ["; - for (int i = 0; i < frame.dlc; i++) { - std::cerr << std::hex << static_cast(frame.data[i]); - if (i < frame.dlc - 1) std::cerr << " "; - } - std::cerr << std::dec << "] - Error: " << strerror(errno) - << " (bytes written: " << nbytes << ")" << std::endl; + std::lock_guard lock(txMutex_); + if (txQueue_.size() >= kMaxTxQueueDepth) { + std::cerr << "CAN TX queue full, dropping frame ID: 0x" << std::hex << frame.id << std::dec << std::endl; return false; } - + txQueue_.push(frame); + txCv_.notify_one(); return true; } +void SocketCanBus::txLoop_() { + while (running_) { + std::unique_lock lock(txMutex_); + txCv_.wait(lock, [this] { + return !txQueue_.empty() || !running_; + }); + + while (!txQueue_.empty()) { + CanFrame frame = txQueue_.front(); + txQueue_.pop(); + lock.unlock(); + + ::can_frame cf; + mapCanFrameToSocketCan(frame, cf); + + int retries = 0; + while (write(socketFd_, &cf, sizeof(cf)) != sizeof(cf)) { + if ((errno == ENOBUFS || errno == EAGAIN) && running_ && retries++ < 10) { + std::this_thread::sleep_for(std::chrono::microseconds(500)); + } else { + std::cerr << "CAN TX failed - ID: 0x" << std::hex << frame.id + << std::dec << " Error: " << strerror(errno) << std::endl; + break; + } + + } + std::this_thread::sleep_for(std::chrono::microseconds(250)); + + + lock.lock(); + } + } +} + void SocketCanBus::receiveLoop_() { ::can_frame can_frame; - + while (running_) { fd_set readfds; struct timeval timeout; - + FD_ZERO(&readfds); FD_SET(socketFd_, &readfds); - + timeout.tv_sec = 0; timeout.tv_usec = 100000; // 100ms timeout - + int result = select(socketFd_ + 1, &readfds, NULL, NULL, &timeout); - + if (result < 0) { if (errno != EINTR) { std::cerr << "SocketCAN select error: " << strerror(errno) << std::endl; @@ -165,15 +185,14 @@ void SocketCanBus::receiveLoop_() { } continue; } - + if (result == 0) { - // Timeout, continue loop continue; } - + if (FD_ISSET(socketFd_, &readfds)) { ssize_t nbytes = read(socketFd_, &can_frame, sizeof(can_frame)); - + if (nbytes == sizeof(can_frame) && receiveCallback_) { CanFrame frame; mapSocketCanToCanFrame(can_frame, frame); diff --git a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml index eb297075..f0d5b832 100644 --- a/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml +++ b/src/subsystems/drive/drive_bringup/config/athena_drive_controllers.yaml @@ -172,5 +172,4 @@ rear_ackermann_controller: track_width: 0.645 wheel_radius: 0.125 max_speed: 0.59 - max_steering_angle: 0.785398 - min_steering_angle: -0.785398 + max_steer_angle: 0.785398 \ No newline at end of file diff --git a/src/subsystems/drive/drive_bringup/config/teleop_twist.yaml b/src/subsystems/drive/drive_bringup/config/teleop_twist.yaml index a40f2da4..e8be3211 100644 --- a/src/subsystems/drive/drive_bringup/config/teleop_twist.yaml +++ b/src/subsystems/drive/drive_bringup/config/teleop_twist.yaml @@ -7,7 +7,7 @@ teleop_twist_joy: axis_linear.z: -1 # unsure why but ackermann controller only goes about half this speed - scale_linear.x: 2.0 + scale_linear.x: 0.59 scale_linear.y: 0.0 scale_linear.z: 0.0 diff --git a/src/subsystems/drive/drive_controllers/CMakeLists.txt b/src/subsystems/drive/drive_controllers/CMakeLists.txt index ec44bb83..20bc96b9 100644 --- a/src/subsystems/drive/drive_controllers/CMakeLists.txt +++ b/src/subsystems/drive/drive_controllers/CMakeLists.txt @@ -22,6 +22,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS find_package(ament_cmake REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(umdloop_can_library REQUIRED) find_package(ament_cmake_gmock REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) @@ -66,7 +67,7 @@ add_library( target_include_directories(drive_controllers PUBLIC "$" "$") -target_link_libraries(drive_controllers swerve_drive_controller_parameters double_ackermann_controller_parameters crab_steering_controller_parameters front_ackermann_controller_parameters rear_ackermann_controller_parameters) +target_link_libraries(drive_controllers swerve_drive_controller_parameters double_ackermann_controller_parameters crab_steering_controller_parameters front_ackermann_controller_parameters rear_ackermann_controller_parameters umdloop_can_library::umdloop_can_library) ament_target_dependencies(drive_controllers ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_compile_definitions(drive_controllers PRIVATE "SWERVE_DRIVE_CONTROLLER_BUILDING_DLL") diff --git a/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml b/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml index b4a7d01b..52438d4d 100644 --- a/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml +++ b/src/subsystems/drive/drive_controllers/config/rear_ackermann_controller.yaml @@ -1,22 +1,14 @@ # Parameters for the RearAckermannController -# -# Joint ordering convention (must be respected in the config): -# steer_joints : [front_left, front_right, rear_left, rear_right] -# front pair → always commanded to 0 (wheels point straight) -# rear pair → Ackermann swerve angles (counter-steer toward ICR) -# drive_joints : [front_left, front_right, rear_left, rear_right] -# front pair → pure Ackermann arc speed -# rear pair → pure Ackermann speed (r * omega) rear_ackermann_controller: - steer_joints: { + drive_joints: { type: string_array, default_value: [], - description: "Steer joint names in order [fl, fr, bl, br]. Front pair held at 0; rear pair gets swerve angles." + description: "The names of the drive wheel joints." } - drive_joints: { + steer_joints: { type: string_array, default_value: [], - description: "Drive joint names in order [fl, fr, bl, br]. Both pairs use Ackermann arc speed." + description: "The names of the steering joints (rear wheels)." } track_width: { type: double, @@ -38,13 +30,8 @@ rear_ackermann_controller: default_value: 1.27, description: "Maximum linear speed in m/s." } - max_steering_angle: { + max_steer_angle: { type: double, - default_value: 0.785398, - description: "Maximum steering angle in radians (pi/4)." - } - min_steering_angle: { - type: double, - default_value: -0.785398, - description: "Minimum steering angle in radians (-pi/4)." - } + default_value: 0.785, + description: "Maximum steering angle in radians." + } \ No newline at end of file diff --git a/src/subsystems/drive/drive_controllers/include/athena_drive_controllers/rear_ackermann_controller.hpp b/src/subsystems/drive/drive_controllers/include/athena_drive_controllers/rear_ackermann_controller.hpp index 8abfd6c6..ab8716ff 100644 --- a/src/subsystems/drive/drive_controllers/include/athena_drive_controllers/rear_ackermann_controller.hpp +++ b/src/subsystems/drive/drive_controllers/include/athena_drive_controllers/rear_ackermann_controller.hpp @@ -30,17 +30,6 @@ namespace drive_controllers { -// RearAckermannController: rear axle steers with Ackermann geometry; front axle uses -// pure Ackermann arc speeds with wheels pointing straight. -// -// Rear axle — swerve/steer: rear joints angle toward the ICR (Ackermann counter-steer -// geometry) and spin at the Ackermann arc speed r * omega. -// -// Front axle — wheels held at 0 (pointing straight); drive speeds use pure -// Ackermann arc speed (r_side * omega / r_w). -// -// ICR reference point: mid-vehicle (wheelbase / 2 ahead of rear axle), consistent with -// double_ackermann_controller. class RearAckermannController : public controller_interface::ControllerInterface { public: @@ -78,8 +67,8 @@ class RearAckermannController : public controller_interface::ControllerInterface std::shared_ptr param_listener_; rear_ackermann_controller::Params params_; - std::vector steer_joint_names_; // [bl, br] - std::vector drive_joint_names_; // [fl, fr, bl, br] + std::vector drive_joint_names_; + std::vector steer_joint_names_; rclcpp::Subscription::SharedPtr ref_subscriber_ = nullptr; realtime_tools::RealtimeBuffer> input_ref_; @@ -91,4 +80,4 @@ class RearAckermannController : public controller_interface::ControllerInterface } // namespace drive_controllers -#endif // ATHENA_DRIVE_CONTROLLERS__REAR_ACKERMANN_CONTROLLER_HPP_ +#endif // ATHENA_DRIVE_CONTROLLERS__REAR_ACKERMANN_CONTROLLER_HPP_ \ No newline at end of file diff --git a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp index 597c8078..d4687968 100644 --- a/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp +++ b/src/subsystems/drive/drive_controllers/src/rear_ackermann_controller.cpp @@ -15,19 +15,18 @@ #include "athena_drive_controllers/rear_ackermann_controller.hpp" #include -#include +#include #include #include #include +#include #include "controller_interface/helpers.hpp" #include "rclcpp/rclcpp.hpp" namespace drive_controllers { - -RearAckermannController::RearAckermannController() -: controller_interface::ControllerInterface() {} +RearAckermannController::RearAckermannController() : controller_interface::ControllerInterface() {} controller_interface::CallbackReturn RearAckermannController::on_init() { @@ -48,8 +47,8 @@ controller_interface::CallbackReturn RearAckermannController::on_configure( const rclcpp_lifecycle::State & /*previous_state*/) { params_ = param_listener_->get_params(); - steer_joint_names_ = params_.steer_joints; // [fl, fr, bl, br] - drive_joint_names_ = params_.drive_joints; // [fl, fr, bl, br] + drive_joint_names_ = params_.drive_joints; + steer_joint_names_ = params_.steer_joints; auto subscribers_qos = rclcpp::SystemDefaultsQoS(); subscribers_qos.keep_last(1); @@ -65,55 +64,52 @@ controller_interface::CallbackReturn RearAckermannController::on_configure( return controller_interface::CallbackReturn::SUCCESS; } -void RearAckermannController::reference_callback( - const std::shared_ptr msg) +void RearAckermannController::reference_callback(const std::shared_ptr msg) { input_ref_.writeFromNonRT(msg); } -controller_interface::InterfaceConfiguration -RearAckermannController::command_interface_configuration() const +controller_interface::InterfaceConfiguration RearAckermannController::command_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - // Steer interfaces: [fl, fr, bl, br] / position - for (const auto & joint : steer_joint_names_) { - config.names.push_back(joint + "/position"); + for (const auto & joint : steer_joint_names_) + { + command_interfaces_config.names.push_back(joint + "/position"); } - // Drive interfaces: [fl, fr, bl, br] / velocity - for (const auto & joint : drive_joint_names_) { - config.names.push_back(joint + "/velocity"); + for (const auto & joint : drive_joint_names_) + { + command_interfaces_config.names.push_back(joint + "/velocity"); } - return config; + return command_interfaces_config; } -controller_interface::InterfaceConfiguration -RearAckermannController::state_interface_configuration() const +controller_interface::InterfaceConfiguration RearAckermannController::state_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::NONE; - return config; + controller_interface::InterfaceConfiguration state_interfaces_config; + state_interfaces_config.type = controller_interface::interface_configuration_type::NONE; + return state_interfaces_config; } controller_interface::CallbackReturn RearAckermannController::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { - for (size_t i = 0; i < command_interfaces_.size(); ++i) { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { command_interfaces_[i].set_value(0.0); } - RCLCPP_INFO( - get_node()->get_logger(), - "RearAckermannController activated with all commands set to zero"); + RCLCPP_INFO(get_node()->get_logger(), "RearAckermannController activated with all commands set to zero"); return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn RearAckermannController::on_deactivate( const rclcpp_lifecycle::State & /*previous_state*/) { - for (size_t i = 0; i < command_interfaces_.size(); ++i) { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { command_interfaces_[i].set_value(0.0); } @@ -124,123 +120,114 @@ controller_interface::return_type RearAckermannController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { auto current_ref = input_ref_.readFromRT(); - if (!current_ref || !(*current_ref)) { - for (size_t i = 0; i < command_interfaces_.size(); ++i) { + if (!current_ref || !(*current_ref)) + { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { command_interfaces_[i].set_value(0.0); } return controller_interface::return_type::OK; } - const double v = std::clamp( + double linear_vel_cmd = std::clamp( (*current_ref)->twist.linear.x, -params_.max_speed, params_.max_speed); - const double omega = (*current_ref)->twist.angular.z; - - const double wheelbase = params_.wheelbase; - const double track_width = params_.track_width; - const double wheel_radius = params_.wheel_radius; - const double half_base = wheelbase / 2.0; - const double half_track = track_width / 2.0; - - // command_interfaces_ layout: - // [0] steer_bl / position ← rear swerve angle - // [1] steer_br / position ← rear swerve angle - // [2] propulsion_fl / velocity ← front Ackermann arc speed - // [3] propulsion_fr / velocity ← front Ackermann arc speed - // [4] propulsion_bl / velocity ← rear Ackermann arc speed - // [5] propulsion_br / velocity ← rear Ackermann arc speed - - if (std::abs(v) < 1e-4) { - // Zero linear velocity: zero all propulsion and return steer to 0 - for (size_t i = 0; i < command_interfaces_.size(); ++i) { - command_interfaces_[i].set_value(0.0); - } - return controller_interface::return_type::OK; + + // Bicycle model: same formula as front steering — sign of steer_cmd determines left vs right + double steer_cmd = 0.0; + if (std::abs(linear_vel_cmd) > 1e-4) { + steer_cmd = std::atan((*current_ref)->twist.angular.z * params_.wheelbase / linear_vel_cmd); } + //steer_cmd = std::clamp(steer_cmd, -params_.max_steer_angle, params_.max_steer_angle); - if (std::abs(omega) < 1e-4) { - // Straight line: all steer = 0, all drive = v / r - const double drive_ang = v / wheel_radius; - command_interfaces_[0].set_value(0.0); - command_interfaces_[1].set_value(0.0); - command_interfaces_[2].set_value(drive_ang); - command_interfaces_[3].set_value(drive_ang); - command_interfaces_[4].set_value(drive_ang); - command_interfaces_[5].set_value(drive_ang); - - const double rad_s_to_rpm = 60.0 / (2.0 * M_PI); - RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 500, - "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f", - drive_ang * rad_s_to_rpm, - drive_ang * rad_s_to_rpm, - drive_ang * rad_s_to_rpm, - drive_ang * rad_s_to_rpm); - return controller_interface::return_type::OK; + double wheelbase = params_.wheelbase; + double track_width = params_.track_width; + double wheel_radius = params_.wheel_radius; + + double rear_left_steer_angle = 0.0; + double rear_right_steer_angle = 0.0; + + double front_left_vel = linear_vel_cmd; + double front_right_vel = linear_vel_cmd; + double rear_left_vel = linear_vel_cmd; + double rear_right_vel = linear_vel_cmd; + + if (std::abs(steer_cmd) > 1e-4) { + double turn_radius = wheelbase / std::tan(steer_cmd); + double angular_vel = std::abs(linear_vel_cmd) / std::abs(turn_radius); + + if (linear_vel_cmd < 0) { + angular_vel = -angular_vel; + } + + double inner_angle = std::atan(wheelbase / (std::abs(turn_radius) - track_width / 2.0)); + double outer_angle = std::atan(wheelbase / (std::abs(turn_radius) + track_width / 2.0)); + + // Rear axle is the steered axle: it traces the longer (front) arc + // Front axle is the fixed axle: it traces the shorter (rear) arc + // Steer angles are negated vs front steering: rear wheels point right to turn left + double inner_rear_vel = angular_vel * (std::abs(turn_radius) - track_width / 2.0); + double outer_rear_vel = angular_vel * (std::abs(turn_radius) + track_width / 2.0); + double inner_front_vel = angular_vel * std::sqrt( + std::pow(wheelbase, 2) + std::pow(std::abs(turn_radius) - track_width / 2.0, 2)); + double outer_front_vel = angular_vel * std::sqrt( + std::pow(wheelbase, 2) + std::pow(std::abs(turn_radius) + track_width / 2.0, 2)); + + if (steer_cmd > 0.0) { // LEFT TURN: left wheel is INNER + rear_left_steer_angle = inner_angle; + rear_right_steer_angle = outer_angle; + + front_left_vel = inner_rear_vel; + front_right_vel = outer_rear_vel; + rear_left_vel = inner_front_vel; + rear_right_vel = outer_front_vel; + + } else { // RIGHT TURN: right wheel is INNER + rear_left_steer_angle = -outer_angle; + rear_right_steer_angle = -inner_angle; + + front_left_vel = outer_rear_vel; + front_right_vel = inner_rear_vel; + rear_left_vel = outer_front_vel; + rear_right_vel = inner_front_vel; + } } - // ── Turn geometry (mid-vehicle ICR, consistent with double_ackermann_controller) ── - // - // ICR is at lateral distance R = v / omega from the vehicle centreline, - // centred between the axles. Left-hand turns have R > 0. - // - // Guard: clamp |turn_radius| to at least (half_track + 0.05 m). - // If the ICR falls inside the wheel track, r_left or r_right changes sign, - // causing a swerve wheel to drive backward and producing erratic behavior - // at low linear speed with higher angular velocity. - const double min_turn_radius = half_track + 0.05; - const double turn_radius = std::copysign( - std::max(std::abs(v / omega), min_turn_radius), v / omega); // R (signed) - const double r_left = turn_radius - half_track; // R − T/2 - const double r_right = turn_radius + half_track; // R + T/2 - - // ── Rear wheels: swerve (steer + Ackermann arc speed) ─────────────────── - // - // Steer angle: rear wheels counter-steer relative to what front wheels would do. - // angle = -atan(half_base / r_side): legs of the right triangle are the longitudinal - // offset (half_base) and the lateral ICR distance (r_side), so atan is correct here. - const double rear_left_steer = std::clamp( - std::atan(half_base / r_right), params_.min_steering_angle, params_.max_steering_angle); - const double rear_right_steer = std::clamp( - std::atan(half_base / r_left), params_.min_steering_angle, params_.max_steering_angle); - - // Arc speed: r * omega (signed — correct for forward and reverse) - // Clamp to max_speed so extreme omega values can't over-command the motors. - const double max_wheel_ang_vel = params_.max_speed / wheel_radius; - const double rear_left_vel = std::clamp( - (r_left * omega) / wheel_radius, -max_wheel_ang_vel, max_wheel_ang_vel); - const double rear_right_vel = std::clamp( - (r_right * omega) / wheel_radius, -max_wheel_ang_vel, max_wheel_ang_vel); - - // ── Front wheels: pure Ackermann arc speed ─────────────────────────────── - // - // The front steer joints are held at 0, so the kinematically correct roll - // speed is r_side * omega / r_w. The inner wheel transitions from forward - // to backward only when |R| < half_track (very tight turns). - const double front_left_vel = std::clamp( - (r_left * omega) / wheel_radius, -max_wheel_ang_vel, max_wheel_ang_vel); - const double front_right_vel = std::clamp( - (r_right * omega) / wheel_radius, -max_wheel_ang_vel, max_wheel_ang_vel); - - command_interfaces_[0].set_value(rear_left_steer); - command_interfaces_[1].set_value(rear_right_steer); - command_interfaces_[2].set_value(front_left_vel); - command_interfaces_[3].set_value(front_right_vel); - command_interfaces_[4].set_value(rear_left_vel); - command_interfaces_[5].set_value(rear_right_vel); + double fl_wheel_ang_vel = front_left_vel / wheel_radius; + double fr_wheel_ang_vel = front_right_vel / wheel_radius; + double rl_wheel_ang_vel = rear_left_vel / wheel_radius; + double rr_wheel_ang_vel = rear_right_vel / wheel_radius; + + // Set steering positions (rear wheels) + + double rear_left_angle_clamped = std::clamp(rear_left_steer_angle, -params_.max_steer_angle, params_.max_steer_angle); + double rear_right_angle_clamped = -1* std::clamp(rear_right_steer_angle, -params_.max_steer_angle, params_.max_steer_angle); + + + + command_interfaces_[0].set_value(rear_left_angle_clamped); + command_interfaces_[1].set_value(rear_right_angle_clamped); + + // Set drive velocities + command_interfaces_[2].set_value(fl_wheel_ang_vel); + command_interfaces_[3].set_value(fr_wheel_ang_vel); + command_interfaces_[4].set_value(rl_wheel_ang_vel); + command_interfaces_[5].set_value(rr_wheel_ang_vel); const double rad_s_to_rpm = 60.0 / (2.0 * M_PI); RCLCPP_INFO_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 500, - "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f", - front_left_vel * rad_s_to_rpm, - front_right_vel * rad_s_to_rpm, - rear_left_vel * rad_s_to_rpm, - rear_right_vel * rad_s_to_rpm); + "Wheel speeds [RPM] - FL: %.2f, FR: %.2f, RL: %.2f, RR: %.2f | Steer [rad] - RL: %.3f, RR: %.3f", + fl_wheel_ang_vel * rad_s_to_rpm, + fr_wheel_ang_vel * rad_s_to_rpm, + rl_wheel_ang_vel * rad_s_to_rpm, + rr_wheel_ang_vel * rad_s_to_rpm, + rear_left_angle_clamped, + rear_right_angle_clamped); return controller_interface::return_type::OK; } - } // namespace drive_controllers #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( - drive_controllers::RearAckermannController, controller_interface::ControllerInterface) + drive_controllers::RearAckermannController, controller_interface::ControllerInterface) \ No newline at end of file diff --git a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml index 7f9a85cf..50fcd0d3 100644 --- a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml +++ b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml @@ -44,7 +44,7 @@ bt_navigator: planner_server: ros__parameters: use_sim_time: false - expected_planner_frequency: 1.0 + expected_planner_frequency: 5.0 planner_plugins: ["GridBased"] GridBased: @@ -54,17 +54,17 @@ planner_server: downsampling_factor: 1 allow_unknown: true max_iterations: 1000000 - max_planning_time: 7.5 + max_planning_time: 2.0 motion_model_for_search: "REEDS_SHEPP" angle_quantization_bins: 72 analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 - minimum_turning_radius: 1.5 + minimum_turning_radius: 2.5 reverse_penalty: 2.5 change_penalty: 0.5 non_straight_penalty: 1.4 - cost_penalty: 2.0 + cost_penalty: 15.0 retrospective_penalty: 0.1 lookup_table_size: 20.0 @@ -76,7 +76,7 @@ controller_server: use_sim_time: false controller_frequency: 20.0 min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.5 + min_y_velocity_threshold: 0.001 # Fixed: was 0.5, too high for non-holonomic robot min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 odom_topic: /localization/odom @@ -97,30 +97,84 @@ controller_server: FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 1.0 + desired_linear_vel: .1524 lookahead_dist: 2.5 - min_lookahead_dist: 1.5 - max_lookahead_dist: 4.0 - lookahead_time: 1.5 - rotate_to_heading_angular_vel: 0.3 - transform_tolerance: 0.5 - use_velocity_scaled_lookahead_dist: true - min_approach_linear_velocity: 0.15 - approach_velocity_scaling_dist: 1.5 - use_collision_detection: true - max_allowed_time_to_collision_up_to_carrot: 1.0 - use_regulated_linear_velocity_scaling: true - use_cost_regulated_linear_velocity_scaling: true - regulated_linear_scaling_min_radius: 1.5 - regulated_linear_scaling_min_speed: 0.25 - use_rotate_to_heading: false - allow_reversing: true - rotate_to_heading_min_angle: 0.785 - max_linear_accel: 1.0 - max_linear_decel: 1.5 - max_angular_accel: 1.2 - max_robot_pose_search_dist: 10.0 - use_interpolation: true + + # Path following + enforce_path_inversion: false + + # Critics — MPPI scores trajectories using these weighted cost functions + critics: + [ + "ConstraintCritic", + "ObstaclesCritic", + "GoalCritic", + "GoalAngleCritic", + "PathAlignCritic", + "PathFollowCritic", + "PathAngleCritic", + "PreferForwardCritic" + ] + + ConstraintCritic: + enabled: true + cost_power: 1 + cost_weight: 4.0 + + ObstaclesCritic: + enabled: true + cost_power: 1 + repulsion_weight: 1.5 + critical_weight: 20.0 # Heavy penalty for trajectories entering obstacles + consider_footprint: true # Use full robot footprint, not point + collision_cost: 10000.0 + collision_margin_distance: 0.15 + near_goal_distance: 0.5 + inflation_radius: 1.2 # Must match local costmap inflation_radius + + GoalCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 1.0 + + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.4 + + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.4 + offset_from_furthest: 20 + use_path_orientations: false + + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 1.4 + + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.4 + max_angle_to_furthest: 1.0 + forward_preference: true + + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 # Penalize reversing unless necessary + threshold_to_consider: 0.4 behavior_server: ros__parameters: @@ -170,19 +224,21 @@ global_costmap: observation_sources: obstacles obstacles: data_type: "PointCloud2" - topic: "/zed/zed_node/point_cloud/cloud_registered_corrected" + topic: "/zed/zed_node/point_cloud/cloud_registered" marking: true clearing: true + sensor_frame: "zed_left_camera_frame" min_obstacle_height: 0.0 max_obstacle_height: 2.0 obstacle_max_range: 10.0 raytrace_max_range: 12.0 obstacle_min_range: 0.0 + observation_persistence: 3.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 1.5 + inflation_radius: 2.0 local_costmap: local_costmap: @@ -193,10 +249,10 @@ local_costmap: update_frequency: 10.0 publish_frequency: 5.0 rolling_window: true - width: 8 - height: 8 + width: 12 + height: 12 resolution: 0.05 - transform_tolerance: 0.3 + transform_tolerance: 1.0 footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]" plugins: ["obstacle_layer", "inflation_layer"] @@ -206,29 +262,30 @@ local_costmap: footprint_clearing_enabled: true min_obstacle_height: 0.0 max_obstacle_height: 2.0 - z_voxels: 10 + z_voxels: 16 origin_z: 0.0 - z_resolution: 0.2 + z_resolution: 0.125 unknown_threshold: 15 - mark_threshold: 0 + mark_threshold: 1 # Fixed: was 0, require 2 filled voxels to mark occupied (reduces noise) combination_method: 1 publish_voxel_map: false observation_sources: obstacles obstacles: data_type: "PointCloud2" - topic: "/zed/zed_node/point_cloud/cloud_registered_corrected" + topic: "/zed/zed_node/point_cloud/cloud_registered" marking: true clearing: true + sensor_frame: "zed_left_camera_frame" min_obstacle_height: 0.0 max_obstacle_height: 2.0 obstacle_max_range: 5.0 - raytrace_max_range: 5.5 + raytrace_max_range: 7.0 obstacle_min_range: 0.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 1.0 - inflation_radius: 1.2 + cost_scaling_factor: 3.5 # Fixed: was 5.0, relaxed to be less aggressive + inflation_radius: 1.5 # Fixed: was 1.2, increased for better obstacle margin lifecycle_manager: ros__parameters: @@ -238,4 +295,4 @@ lifecycle_manager: - controller_server - planner_server - behavior_server - - bt_navigator \ No newline at end of file + - bt_navigator diff --git a/src/tools/scripts/add_internet.sh b/src/tools/scripts/add_internet.sh new file mode 100755 index 00000000..a96abcb8 --- /dev/null +++ b/src/tools/scripts/add_internet.sh @@ -0,0 +1,3 @@ +sudo ip route add default via 192.168.88.10 +echo "nameserver 8.8.8.8" | sudo tee /etc/resolv.conf + diff --git a/src/tools/scripts/can_test.sh b/src/tools/scripts/can_test.sh new file mode 100755 index 00000000..bc47f489 --- /dev/null +++ b/src/tools/scripts/can_test.sh @@ -0,0 +1,160 @@ +#!/usr/bin/env bash +# can_error_parse.sh — Live CAN error frame decoder +# Usage: candump can0 -e -t a | ./can_error_parse.sh + +RED='\033[0;31m'; YEL='\033[0;33m'; CYN='\033[0;36m' +GRN='\033[0;32m'; DIM='\033[2m'; RST='\033[0m' + +decode_error_class() { + local b=$((16#$1)) + local msgs=() + (( b & 0x01 )) && msgs+=("TX timeout") + (( b & 0x02 )) && msgs+=("Lost arbitration") + (( b & 0x04 )) && msgs+=("Bus error") + (( b & 0x08 )) && msgs+=("Controller error") + (( b & 0x10 )) && msgs+=("Protocol violation") + (( b & 0x20 )) && msgs+=("Transceiver error") + (( b & 0x40 )) && msgs+=("No ACK on TX") + (( b & 0x80 )) && msgs+=("RX error counter changed") + printf '%s' "${msgs[*]:-(unknown)}" +} + +decode_controller() { + local b=$((16#$1)) + local msgs=() + (( b & 0x01 )) && msgs+=("RX buf overflow (unspec)") + (( b & 0x02 )) && msgs+=("RX buf 0 overflow") + (( b & 0x04 )) && msgs+=("RX buf 1 overflow") + (( b & 0x08 )) && msgs+=("TX buf overflow") + (( b & 0x10 )) && msgs+=("RX warning (≥96 errors)") + (( b & 0x20 )) && msgs+=("TX warning (≥96 errors)") + (( b & 0x40 )) && msgs+=("RX passive (≥128 errors) — error-active recovery") + (( b & 0x80 )) && msgs+=("TX passive (≥128 errors) — error-passive state") + printf '%s' "${msgs[*]:-(none)}" +} + +decode_protocol() { + local b=$((16#$1)) + local msgs=() + (( b & 0x01 )) && msgs+=("single bit") + (( b & 0x02 )) && msgs+=("frame format") + (( b & 0x04 )) && msgs+=("bit stuffing") + (( b & 0x08 )) && msgs+=("dominant bit (0 expected)") + (( b & 0x10 )) && msgs+=("recessive bit (1 expected)") + (( b & 0x20 )) && msgs+=("bus overload") + (( b & 0x40 )) && msgs+=("active error announcement") + (( b & 0x80 )) && msgs+=("TX while RX in progress") + printf '%s' "${msgs[*]:-(none)}" +} + +decode_location() { + case "${1^^}" in + 00) printf 'unspecified' ;; + 02) printf 'ID bits 28-21' ;; + 03) printf 'Start of Frame' ;; + 04) printf 'bit SRTR' ;; + 05) printf 'bit IDE' ;; + 06) printf 'ID bits 17-13' ;; + 07) printf 'ID bits 12-5' ;; + 08) printf 'CRC sequence' ;; + 09) printf 'reserved bit 0' ;; + 0A) printf 'Data section' ;; + 0B) printf 'Data length code' ;; + 0C) printf 'RTR bit' ;; + 0D) printf 'reserved bit 1' ;; + 0E) printf 'ID bits 4-0' ;; + 0F) printf 'ID bits 20-18' ;; + 12) printf 'Intermission' ;; + 13) printf 'ACK slot' ;; + 14) printf 'EOF' ;; + 15) printf 'ACK delimiter' ;; + 16) printf 'CRC delimiter' ;; + 17) printf 'SOF' ;; + 18) printf 'Error delimiter' ;; + 19) printf 'Overload flag' ;; + 1A) printf 'Active error flag' ;; + 1B) printf 'Passive error flag' ;; + *) printf "loc=0x$1" ;; + esac +} + +decode_transceiver() { + case "${1^^}" in + 00) printf 'OK' ;; + 04) printf 'CANH: no wire' ;; + 05) printf 'CANH: short to BAT' ;; + 06) printf 'CANH: short to VCC' ;; + 07) printf 'CANH: short to GND' ;; + 40) printf 'CANL: no wire' ;; + 50) printf 'CANL: short to BAT' ;; + 60) printf 'CANL: short to VCC' ;; + 70) printf 'CANL: short to GND' ;; + 80) printf 'CANL/H: short together' ;; + *) printf "trx=0x$1" ;; + esac +} + +# Severity label based on which class bits are set +severity_label() { + local b=$((16#$1)) + if (( b & 0x40 )); then echo -e "${RED}[BUS OFF]${RST}" + elif (( b & 0x20 )); then echo -e "${RED}[ERROR-PASSIVE]${RST}" + elif (( b & 0x04 || b & 0x08 )); then echo -e "${YEL}[ERROR-ACTIVE]${RST}" + else echo -e "${DIM}[INFO]${RST}" + fi +} + +# ── candump -e -t a line format: +# (1234.567890) can0 0AC [8] 00 04 00 00 00 00 00 00 +# ^timestamp ^iface ^id ^dlc ^b0 b1 b2 b3 b4 b5 b6 b7 + +while IFS= read -r line; do + # Strip timestamp (leading word in parens) + clean=$(sed 's/([0-9]*\.[0-9]*) //' <<< "$line") + + read -r iface frameid dlc b0 b1 b2 b3 b4 b5 b6 b7 <<< "$clean" + + # Match only error frame IDs — 3 hex digits, no colon (not data frames) + # Error frames never have '#' in candump -e output + [[ "$frameid" =~ ^[0-9A-Fa-f]{3,8}$ ]] || continue + [[ "$dlc" == "[8]" ]] || continue + [[ -z "$b0" ]] && continue + + id_val=$((16#${frameid})) + + # Error frame IDs from the kernel always have bit 29 set in the 32-bit word. + # candump may show the full value (20000004) or just the class byte (AC). + # Normalise: if value > 0x1FF it's a full 29-bit ID; mask off the top. + # If it's ≤ 0xFF it's already the class byte displayed short. + if (( id_val > 0x1FFFFFFF )); then continue # not a valid CAN ID + elif (( id_val > 0xFFF )); then err_class=$(( id_val & 0xFF )) + else err_class=$id_val + fi + + class_hex=$(printf '%02X' $err_class) + sev=$(severity_label "$class_hex") + + echo -e "${CYN}───────────────────────────────────────────────────────${RST}" + printf "${RED}⚠ ERROR FRAME${RST} %s iface=${GRN}%s${RST} id=0x%s class=0x%s\n" \ + "$sev" "$iface" "$frameid" "$class_hex" + + echo -e " ${YEL}Error class :${RST} $(decode_error_class "$class_hex")" + + [[ -n "$b1" && "$b1" != "00" ]] && \ + echo -e " ${YEL}Controller :${RST} $(decode_controller "$b1")" + [[ -n "$b2" && "$b2" != "00" ]] && \ + echo -e " ${YEL}Protocol :${RST} $(decode_protocol "$b2")" + [[ -n "$b3" && "$b3" != "00" ]] && \ + echo -e " ${YEL}Location :${RST} $(decode_location "$b3")" + [[ -n "$b4" && "$b4" != "00" ]] && \ + echo -e " ${YEL}Transceiver :${RST} $(decode_transceiver "$b4")" + + # Bytes 6 & 7 are TX/RX error counters + txcnt=$((16#${b6:-00})); rxcnt=$((16#${b7:-00})) + (( txcnt > 0 )) && echo -e " ${YEL}TX err count:${RST} $txcnt" + (( rxcnt > 0 )) && echo -e " ${YEL}RX err count:${RST} $rxcnt" + + echo -e " ${DIM}Raw bytes: $b0 $b1 $b2 $b3 $b4 $b5 $b6 $b7${RST}" + +done + diff --git a/src/tools/scripts/jetson_canable.sh b/src/tools/scripts/jetson_canable.sh new file mode 100755 index 00000000..7cd5924a --- /dev/null +++ b/src/tools/scripts/jetson_canable.sh @@ -0,0 +1,7 @@ +#!/bin/bash +sudo ip link set can1 up type can \ + bitrate 1000000 \ + sjw 10 \ + restart-ms 1000 + +sudo ifconfig can1 txqueuelen 1000 diff --git a/src/tools/scripts/nav2_lifecycle_status.sh b/src/tools/scripts/nav2_lifecycle_status.sh old mode 100755 new mode 100644 diff --git a/src/tools/scripts/send_gps_goal.sh b/src/tools/scripts/send_gps_goal.sh old mode 100755 new mode 100644 index ba83d542..e69de29b --- a/src/tools/scripts/send_gps_goal.sh +++ b/src/tools/scripts/send_gps_goal.sh @@ -1,43 +0,0 @@ -#!/usr/bin/env bash -# Send a GPS navigation goal to the navigate_to_gps action server. -# -# Usage: -# ./send_gps_goal.sh [tolerance_m] -# -# Examples: -# ./send_gps_goal.sh 38.423911 -110.784905 -# ./send_gps_goal.sh 38.423911 -110.784905 3.0 - -set -e - -usage() { - echo "Usage: $0 [tolerance_m]" - echo "" - echo " latitude Target latitude in decimal degrees" - echo " longitude Target longitude in decimal degrees" - echo " tolerance_m Position tolerance in metres (default: 0.0 = use server default)" - exit 1 -} - -if [[ $# -lt 2 ]]; then - usage -fi - -LAT="$1" -LON="$2" -TOLERANCE="${3:-0.0}" - -echo "==========================================" -echo " GPS Goal" -echo "==========================================" -echo " Latitude : $LAT" -echo " Longitude : $LON" -echo " Tolerance : $TOLERANCE m" -echo "==========================================" -echo "" - -ros2 action send_goal \ - --feedback \ - /navigate_to_gps \ - msgs/action/NavigateToGPS \ - "{latitude: $LAT, longitude: $LON, position_tolerance: $TOLERANCE}" From f9f01e1c765e8886374fe3acc73c481dee5fdef9 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 7 Apr 2026 09:34:54 -0400 Subject: [PATCH 2/3] revert nav2 params and hopefully fix build issues --- .../drive/drive_controllers/package.xml | 1 + .../athena_planner/config/nav2_params.yaml | 139 ++++++------------ 2 files changed, 42 insertions(+), 98 deletions(-) diff --git a/src/subsystems/drive/drive_controllers/package.xml b/src/subsystems/drive/drive_controllers/package.xml index 385ee5f0..cbca0ab3 100644 --- a/src/subsystems/drive/drive_controllers/package.xml +++ b/src/subsystems/drive/drive_controllers/package.xml @@ -13,6 +13,7 @@ control_msgs umdloop_can + umdloop_can_library geometry_msgs controller_interface hardware_interface diff --git a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml index 50fcd0d3..7f9a85cf 100644 --- a/src/subsystems/navigation/athena_planner/config/nav2_params.yaml +++ b/src/subsystems/navigation/athena_planner/config/nav2_params.yaml @@ -44,7 +44,7 @@ bt_navigator: planner_server: ros__parameters: use_sim_time: false - expected_planner_frequency: 5.0 + expected_planner_frequency: 1.0 planner_plugins: ["GridBased"] GridBased: @@ -54,17 +54,17 @@ planner_server: downsampling_factor: 1 allow_unknown: true max_iterations: 1000000 - max_planning_time: 2.0 + max_planning_time: 7.5 motion_model_for_search: "REEDS_SHEPP" angle_quantization_bins: 72 analytic_expansion_ratio: 3.5 analytic_expansion_max_length: 3.0 - minimum_turning_radius: 2.5 + minimum_turning_radius: 1.5 reverse_penalty: 2.5 change_penalty: 0.5 non_straight_penalty: 1.4 - cost_penalty: 15.0 + cost_penalty: 2.0 retrospective_penalty: 0.1 lookup_table_size: 20.0 @@ -76,7 +76,7 @@ controller_server: use_sim_time: false controller_frequency: 20.0 min_x_velocity_threshold: 0.001 - min_y_velocity_threshold: 0.001 # Fixed: was 0.5, too high for non-holonomic robot + min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 odom_topic: /localization/odom @@ -97,84 +97,30 @@ controller_server: FollowPath: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: .1524 + desired_linear_vel: 1.0 lookahead_dist: 2.5 - - # Path following - enforce_path_inversion: false - - # Critics — MPPI scores trajectories using these weighted cost functions - critics: - [ - "ConstraintCritic", - "ObstaclesCritic", - "GoalCritic", - "GoalAngleCritic", - "PathAlignCritic", - "PathFollowCritic", - "PathAngleCritic", - "PreferForwardCritic" - ] - - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - - ObstaclesCritic: - enabled: true - cost_power: 1 - repulsion_weight: 1.5 - critical_weight: 20.0 # Heavy penalty for trajectories entering obstacles - consider_footprint: true # Use full robot footprint, not point - collision_cost: 10000.0 - collision_margin_distance: 0.15 - near_goal_distance: 0.5 - inflation_radius: 1.2 # Must match local costmap inflation_radius - - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.0 - - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.4 - - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 3 - threshold_to_consider: 0.4 - offset_from_furthest: 20 - use_path_orientations: false - - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.4 - max_angle_to_furthest: 1.0 - forward_preference: true - - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 # Penalize reversing unless necessary - threshold_to_consider: 0.4 + min_lookahead_dist: 1.5 + max_lookahead_dist: 4.0 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 0.3 + transform_tolerance: 0.5 + use_velocity_scaled_lookahead_dist: true + min_approach_linear_velocity: 0.15 + approach_velocity_scaling_dist: 1.5 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: true + regulated_linear_scaling_min_radius: 1.5 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: false + allow_reversing: true + rotate_to_heading_min_angle: 0.785 + max_linear_accel: 1.0 + max_linear_decel: 1.5 + max_angular_accel: 1.2 + max_robot_pose_search_dist: 10.0 + use_interpolation: true behavior_server: ros__parameters: @@ -224,21 +170,19 @@ global_costmap: observation_sources: obstacles obstacles: data_type: "PointCloud2" - topic: "/zed/zed_node/point_cloud/cloud_registered" + topic: "/zed/zed_node/point_cloud/cloud_registered_corrected" marking: true clearing: true - sensor_frame: "zed_left_camera_frame" min_obstacle_height: 0.0 max_obstacle_height: 2.0 obstacle_max_range: 10.0 raytrace_max_range: 12.0 obstacle_min_range: 0.0 - observation_persistence: 3.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 2.0 + inflation_radius: 1.5 local_costmap: local_costmap: @@ -249,10 +193,10 @@ local_costmap: update_frequency: 10.0 publish_frequency: 5.0 rolling_window: true - width: 12 - height: 12 + width: 8 + height: 8 resolution: 0.05 - transform_tolerance: 1.0 + transform_tolerance: 0.3 footprint: "[[0.45, 0.33], [0.45, -0.33], [-0.35, -0.33], [-0.35, 0.33]]" plugins: ["obstacle_layer", "inflation_layer"] @@ -262,30 +206,29 @@ local_costmap: footprint_clearing_enabled: true min_obstacle_height: 0.0 max_obstacle_height: 2.0 - z_voxels: 16 + z_voxels: 10 origin_z: 0.0 - z_resolution: 0.125 + z_resolution: 0.2 unknown_threshold: 15 - mark_threshold: 1 # Fixed: was 0, require 2 filled voxels to mark occupied (reduces noise) + mark_threshold: 0 combination_method: 1 publish_voxel_map: false observation_sources: obstacles obstacles: data_type: "PointCloud2" - topic: "/zed/zed_node/point_cloud/cloud_registered" + topic: "/zed/zed_node/point_cloud/cloud_registered_corrected" marking: true clearing: true - sensor_frame: "zed_left_camera_frame" min_obstacle_height: 0.0 max_obstacle_height: 2.0 obstacle_max_range: 5.0 - raytrace_max_range: 7.0 + raytrace_max_range: 5.5 obstacle_min_range: 0.0 inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 3.5 # Fixed: was 5.0, relaxed to be less aggressive - inflation_radius: 1.5 # Fixed: was 1.2, increased for better obstacle margin + cost_scaling_factor: 1.0 + inflation_radius: 1.2 lifecycle_manager: ros__parameters: @@ -295,4 +238,4 @@ lifecycle_manager: - controller_server - planner_server - behavior_server - - bt_navigator + - bt_navigator \ No newline at end of file From d20a715fd17a6f1c6c57705b68410fd7548ef358 Mon Sep 17 00:00:00 2001 From: Mohammad Durrani Date: Tue, 7 Apr 2026 09:41:51 -0400 Subject: [PATCH 3/3] removed incorrect cyclonedds config --- src/config/cyclonedds/config_base.xml | 23 --------------------- src/config/cyclonedds/config_right_base.xml | 4 ++-- 2 files changed, 2 insertions(+), 25 deletions(-) delete mode 100644 src/config/cyclonedds/config_base.xml diff --git a/src/config/cyclonedds/config_base.xml b/src/config/cyclonedds/config_base.xml deleted file mode 100644 index 7d6f6d0d..00000000 --- a/src/config/cyclonedds/config_base.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - false - - - - - - - auto - 1000 - - - - - - diff --git a/src/config/cyclonedds/config_right_base.xml b/src/config/cyclonedds/config_right_base.xml index b6fa70fe..7d6f6d0d 100644 --- a/src/config/cyclonedds/config_right_base.xml +++ b/src/config/cyclonedds/config_right_base.xml @@ -10,8 +10,8 @@ https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclo - - + + auto 1000