From e0d7903dadc4d900c23020823254b93160d73644 Mon Sep 17 00:00:00 2001 From: DeeKayG Date: Wed, 25 Oct 2023 13:25:35 -0400 Subject: [PATCH] Added DBW Bridge to new Branch --- src/vehicle/external/pacmod_interface | 1 + .../universe_dbw_bridge/CMakeLists.txt | 35 ++++ .../external/universe_dbw_bridge/package.xml | 24 +++ .../src/Lincoln_MKZ_Bridge.cpp | 194 ++++++++++++++++++ 4 files changed, 254 insertions(+) create mode 160000 src/vehicle/external/pacmod_interface create mode 100644 src/vehicle/external/universe_dbw_bridge/CMakeLists.txt create mode 100644 src/vehicle/external/universe_dbw_bridge/package.xml create mode 100644 src/vehicle/external/universe_dbw_bridge/src/Lincoln_MKZ_Bridge.cpp diff --git a/src/vehicle/external/pacmod_interface b/src/vehicle/external/pacmod_interface new file mode 160000 index 00000000000..664a58db456 --- /dev/null +++ b/src/vehicle/external/pacmod_interface @@ -0,0 +1 @@ +Subproject commit 664a58db456e092659e5b25954fbf525593bf10d diff --git a/src/vehicle/external/universe_dbw_bridge/CMakeLists.txt b/src/vehicle/external/universe_dbw_bridge/CMakeLists.txt new file mode 100644 index 00000000000..f840b1534a7 --- /dev/null +++ b/src/vehicle/external/universe_dbw_bridge/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.8) +project(universe_dbw_bridge) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(autoware_auto_vehicle_msgs REQUIRED) +find_package(autoware_auto_control_msgs REQUIRED) +find_package(dbw_ford_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +add_executable(Lincoln_MKZ_Bridge src/Lincoln_MKZ_Bridge.cpp) +ament_target_dependencies(Lincoln_MKZ_Bridge rclcpp autoware_auto_vehicle_msgs autoware_auto_control_msgs dbw_ford_msgs sensor_msgs) + +install(TARGETS + Lincoln_MKZ_Bridge + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/vehicle/external/universe_dbw_bridge/package.xml b/src/vehicle/external/universe_dbw_bridge/package.xml new file mode 100644 index 00000000000..dedb3b4eae3 --- /dev/null +++ b/src/vehicle/external/universe_dbw_bridge/package.xml @@ -0,0 +1,24 @@ + + + + universe_dbw_bridge + 1.0.0 + TODO: Package description + DeeKay Goswami + TODO: License declaration + + ament_cmake + + rclcpp + autoware_auto_vehicle_msgs + autoware_auto_control_msgs + dbw_ford_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/vehicle/external/universe_dbw_bridge/src/Lincoln_MKZ_Bridge.cpp b/src/vehicle/external/universe_dbw_bridge/src/Lincoln_MKZ_Bridge.cpp new file mode 100644 index 00000000000..95e2faff54c --- /dev/null +++ b/src/vehicle/external/universe_dbw_bridge/src/Lincoln_MKZ_Bridge.cpp @@ -0,0 +1,194 @@ +// Author: DeeKay Goswami + +#include "rclcpp/rclcpp.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" + +#include "dbw_ford_msgs/msg/gear_cmd.hpp" +#include "dbw_ford_msgs/msg/brake_cmd.hpp" +#include "dbw_ford_msgs/msg/throttle_cmd.hpp" +#include "dbw_ford_msgs/msg/steering_cmd.hpp" +#include "dbw_ford_msgs/msg/gear_report.hpp" +#include "dbw_ford_msgs/msg/steering_report.hpp" +#include "sensor_msgs/msg/joy.hpp" + +class Lincoln_MKZ_Bridge : public rclcpp::Node +{ +public: + Lincoln_MKZ_Bridge() : Node("Lincoln_MKZ_Bridge") + { + //Output to DBW Node + gear_publisher_ = this->create_publisher("/vehicle/gear_cmd", 10); + throttle_publisher_ = this->create_publisher("/vehicle/throttle_cmd", 10); + brake_publisher_ = this->create_publisher("/vehicle/brake_cmd", 10); + steering_publisher_ = this->create_publisher("/vehicle/steering_cmd", 10); + + //Output to Autoware + gear_report_publisher_ = this->create_publisher("/vehicle/status/gear_status", 10); + steering_report_publisher_ = this->create_publisher("/vehicle/status/steering_status", 10); + + //Input from DBW Node + gear_report_subscription_ = this->create_subscription( + "/vehicle/gear_report", 10, + std::bind(&Lincoln_MKZ_Bridge::gearReportCallback, this, std::placeholders::_1)); + steering_report_subscription_ = this->create_subscription( + "/vehicle/steering_report", 10, + std::bind(&Lincoln_MKZ_Bridge::steeringReportCallback, this, std::placeholders::_1)); + + //Input from Autoware + gear_subscription_ = this->create_subscription( + "/control/command/gear_cmd", 10, + std::bind(&Lincoln_MKZ_Bridge::gearCallback, this, std::placeholders::_1)); + control_subscription_ = this->create_subscription( + "/control/command/control_cmd", 10, + std::bind(&Lincoln_MKZ_Bridge::controlCallback, this, std::placeholders::_1)); + + // Joystick subscription + sub_joy_ = this->create_subscription("/joy", 10, std::bind(&Lincoln_MKZ_Bridge::recvJoy, this, std::placeholders::_1)); + } + +private: + + // Joystick variables + sensor_msgs::msg::Joy joy_; + const size_t AXIS_THROTTLE = 1; + const size_t AXIS_BRAKE = 2; + const size_t AXIS_STEER = 3; + bool joy_throttle_valid = false; + bool joy_brake_valid = false; + + void gearCallback(const autoware_auto_vehicle_msgs::msg::GearCommand::SharedPtr msg) + { + dbw_ford_msgs::msg::GearCmd ford_msg; + ford_msg.cmd.gear = translate_gear(msg->command); + gear_publisher_->publish(ford_msg); + } + + void controlCallback(const autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) + { + // Throttle + dbw_ford_msgs::msg::ThrottleCmd throttle_msg; + throttle_msg.pedal_cmd = msg->longitudinal.speed; + if(joy_throttle_valid) { + throttle_msg.pedal_cmd += 0.5 - 0.5 * joy_.axes[AXIS_THROTTLE]; + } + throttle_msg.pedal_cmd_type = dbw_ford_msgs::msg::ThrottleCmd::CMD_PERCENT; + throttle_msg.enable = true; + throttle_publisher_->publish(throttle_msg); + + // Brake + float brake_value = msg->longitudinal.acceleration < 0 ? -msg->longitudinal.acceleration : 0; + if(joy_brake_valid) { + brake_value += 0.5 - 0.5 * joy_.axes[AXIS_BRAKE]; + } + if(brake_value > 0) { + dbw_ford_msgs::msg::BrakeCmd brake_msg; + brake_msg.pedal_cmd = brake_value; + brake_msg.pedal_cmd_type = dbw_ford_msgs::msg::BrakeCmd::CMD_PERCENT; + brake_msg.enable = true; + brake_publisher_->publish(brake_msg); + } + + // Steering + dbw_ford_msgs::msg::SteeringCmd steering_msg; + steering_msg.steering_wheel_angle_cmd = msg->lateral.steering_tire_angle; + if(joy_.axes.size() > AXIS_STEER) { + steering_msg.steering_wheel_angle_cmd += joy_.axes[AXIS_STEER]; + } + steering_msg.steering_wheel_angle_velocity = msg->lateral.steering_tire_rotation_rate; + steering_msg.cmd_type = dbw_ford_msgs::msg::SteeringCmd::CMD_ANGLE; + steering_msg.enable = true; + steering_publisher_->publish(steering_msg); + } + + uint8_t translate_gear(uint8_t autoware_gear) + { + using FordGear = dbw_ford_msgs::msg::Gear; + + switch(autoware_gear) + { + case autoware_auto_vehicle_msgs::msg::GearCommand::NEUTRAL: return FordGear::NEUTRAL; + case autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE: + case autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE_2: + return FordGear::DRIVE; + case autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE: + case autoware_auto_vehicle_msgs::msg::GearCommand::REVERSE_2: + return FordGear::REVERSE; + case autoware_auto_vehicle_msgs::msg::GearCommand::PARK: return FordGear::PARK; + case autoware_auto_vehicle_msgs::msg::GearCommand::LOW: + case autoware_auto_vehicle_msgs::msg::GearCommand::LOW_2: + return FordGear::LOW; + default: return FordGear::NONE; + } + } + + void gearReportCallback(const dbw_ford_msgs::msg::GearReport::SharedPtr msg) + { + autoware_auto_vehicle_msgs::msg::GearReport aw_report; + aw_report.stamp = msg->header.stamp; + aw_report.report = translateFordGearToAutowareGear(msg->state.gear); + gear_report_publisher_->publish(aw_report); + } + + uint8_t translateFordGearToAutowareGear(uint8_t ford_gear) + { + using AutowareGear = autoware_auto_vehicle_msgs::msg::GearReport; + + switch(ford_gear) + { + case dbw_ford_msgs::msg::Gear::PARK: return AutowareGear::PARK; + case dbw_ford_msgs::msg::Gear::REVERSE: return AutowareGear::REVERSE; + case dbw_ford_msgs::msg::Gear::NEUTRAL: return AutowareGear::NEUTRAL; + case dbw_ford_msgs::msg::Gear::DRIVE: return AutowareGear::DRIVE; + case dbw_ford_msgs::msg::Gear::LOW: return AutowareGear::LOW; + default: return AutowareGear::NONE; + } + } + + void steeringReportCallback(const dbw_ford_msgs::msg::SteeringReport::SharedPtr msg) + { + autoware_auto_vehicle_msgs::msg::SteeringReport aw_steering_report; + aw_steering_report.stamp = msg->header.stamp; + aw_steering_report.steering_tire_angle = msg->steering_wheel_angle; + steering_report_publisher_->publish(aw_steering_report); + } + + // Joystick callback + void recvJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) + { + // Check for valid axes + if (msg->axes.size() <= AXIS_STEER) return; + + // Handle joystick values + joy_throttle_valid = msg->axes[AXIS_THROTTLE] != 0.0; + joy_brake_valid = msg->axes[AXIS_BRAKE] != 0.0; + + // Save the received message + joy_ = *msg; + } + + rclcpp::Subscription::SharedPtr gear_subscription_; + rclcpp::Subscription::SharedPtr control_subscription_; + rclcpp::Subscription::SharedPtr gear_report_subscription_; + rclcpp::Subscription::SharedPtr steering_report_subscription_; + rclcpp::Subscription::SharedPtr sub_joy_; + + rclcpp::Publisher::SharedPtr steering_report_publisher_; + rclcpp::Publisher::SharedPtr gear_report_publisher_; + rclcpp::Publisher::SharedPtr gear_publisher_; + rclcpp::Publisher::SharedPtr throttle_publisher_; + rclcpp::Publisher::SharedPtr brake_publisher_; + rclcpp::Publisher::SharedPtr steering_publisher_; + +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}