diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..0357ab0 --- /dev/null +++ b/.clang-format @@ -0,0 +1,5 @@ +BasedOnStyle: Google +DerivePointerAlignment: false +PointerAlignment: Left +IndentWidth: 4 +ColumnLimit: 100 \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 65f5622..b101102 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,7 +17,7 @@ IF(CMAKE_VERSION VERSION_LESS "3.7.0") SET(CMAKE_INCLUDE_CURRENT_DIR ON) ENDIF() -SET(CMAKE_CXX_FLAGS "-std=c++11 -march=native -Werror -Wall -g") +SET(CMAKE_CXX_FLAGS "-std=c++11 -march=native -Werror -Wall -Wextra -g") IF(${CMAKE_BUILD_TYPE} MATCHES "Release") MESSAGE(STATUS "Additional Flags for Release mode") @@ -90,7 +90,8 @@ ENDIF() IF(${CMAKE_BUILD_MODE} MATCHES "Omega") ROSBUILD_ADD_EXECUTABLE(radio_driver src/radio_driver/radio_driver_node.cpp - src/radio_driver/radio_driver.cpp) + src/radio_driver/radio_driver.cpp + src/radio_driver/radio_interface.cpp) TARGET_LINK_LIBRARIES(radio_driver ${libs}) ENDIF() diff --git a/src/radio_driver/radio_driver.cpp b/src/radio_driver/radio_driver.cpp index 33c24e5..92276d8 100644 --- a/src/radio_driver/radio_driver.cpp +++ b/src/radio_driver/radio_driver.cpp @@ -1,26 +1,140 @@ #include "radio_driver/radio_driver.h" +#include +#include +#include + +#include "math/math_util.h" + static const bool kDebug = false; + +// TODO: move these to config static const float kCommandRate = 20; static const float kCommandInterval = 1.0 / kCommandRate; +static const float kCommandTimeout = 0.5; +static const float kFullSpeed = 8.0; // the fastest the car is capable of going at full throttle +static const float kMaxSpeed = 2.0; // the fastest we set the caw to be able to go +static const float kSpeedToThrottle = 1.0 / kFullSpeed; // speed * kSpeedToThrottle = throttle +static const float kMaxAccel = 6.0; +static const float kMaxDecel = 6.0; +static const float kMaxCurvature = 1.5; +static const float kCurvatureToSteering = 1.0 / kMaxCurvature; namespace radio_driver { -RadioDriver::RadioDriver(ros::NodeHandle nh) { +static geometry_msgs::TwistStamped generateTwist(float velocity, float curvature) { + geometry_msgs::TwistStamped twist_msg; + twist_msg.header.stamp = ros::Time::now(); + twist_msg.twist.linear.x = velocity; + twist_msg.twist.linear.y = 0; + twist_msg.twist.linear.z = 0; + twist_msg.twist.angular.x = 0; + twist_msg.twist.angular.y = 0; + twist_msg.twist.angular.z = velocity * curvature; + return twist_msg; +} + +RadioDriver::RadioDriver(ros::NodeHandle nh) : drive_mode_(DriveMode::Disabled) { // TODO: init radio device - joystick_sub_ = - nh.subscribe("/joystick", 10, &RadioDriver::joystickCallback, this); - timer_ = nh.createSteadyTimer(ros::WallDuration(kCommandInterval), - &RadioDriver::timerCallback, this); + odom_pub_ = nh.advertise("odom", 1); + drive_pub_ = nh.advertise("radio_drive", 1); + autonomy_enabler_pub_ = nh.advertise("autonomy_enabler", 1); + + joystick_sub_ = nh.subscribe("/joystick", 10, &RadioDriver::joystickCallback, this); + timer_ = nh.createSteadyTimer(ros::WallDuration(kCommandInterval), &RadioDriver::timerCallback, + this); +} + +void RadioDriver::ackermannCurvatureCallback(const amrl_msgs::AckermannCurvatureDriveMsg& msg) { + ackermann_velocity_ = msg.velocity; + ackermann_curvature_ = msg.curvature; } void RadioDriver::joystickCallback(const sensor_msgs::Joy& msg) { // TODO: track this state + static const size_t kManualDriveButton = 4; + static const size_t kAutonomousDriveButton = 5; + + if (msg.buttons[kManualDriveButton]) { + drive_mode_ = DriveMode::Manual; + } else if (msg.buttons[kAutonomousDriveButton]) { + drive_mode_ = DriveMode::Autonomous; + } else { + drive_mode_ = DriveMode::Disabled; + } + + // TODO: make this configurable + // TODO: add turbo + // TODO: check this is the right range + joystick_velocity_ = -msg.axes[4] * kMaxSpeed; + joystick_curvature_ = -msg.axes[0] * kMaxCurvature; + + static std_msgs::Bool autonomy_enabler_msg; + autonomy_enabler_msg.data = + drive_mode_ == DriveMode::Autonomous || drive_mode_ == DriveMode::ContinuousAutonomous; + autonomy_enabler_pub_.publish(autonomy_enabler_msg); } void RadioDriver::timerCallback(const ros::SteadyTimerEvent& event) { - // TODO: publish commands + static float throttle, steering; + + if (!radio_interface_.isConnected()) { + return; + } + + float vel = 0; + float curv = 0; + switch (drive_mode_) { + case DriveMode::Disabled: { + vel = 0; + curv = 0; + break; + } + case DriveMode::Manual: { + // set throttle and steering based on joystick state + vel = clampVelocity(joystick_velocity_); + curv = clampCurvature(joystick_curvature_); + break; + } + case DriveMode::Autonomous: { + // set throttle and steering based on ackermann commands + vel = clampVelocity(ackermann_velocity_); + curv = clampCurvature(ackermann_curvature_); + break; + } + case DriveMode::ContinuousAutonomous: { + // set throttle and steering based on ackermann commands + vel = clampVelocity(ackermann_velocity_); + curv = clampCurvature(ackermann_curvature_); + break; + } + } + + // TODO: make sure this linearity is real + throttle = vel * kSpeedToThrottle; + steering = curv * kCurvatureToSteering; + + // TODO: odometry updates + radio_interface_.sendControl(throttle, steering); + drive_pub_.publish(generateTwist(vel, curv)); + + lastThrottle_ = throttle; + lastSteering_ = steering; + lastSpeed_ = vel; + lastCurvature_ = curv; +} + +float RadioDriver::clampVelocity(float velocity) const { + const float max_accel = lastSpeed_ > 0 ? kMaxAccel : kMaxDecel; + const float max_decel = lastSpeed_ > 0 ? kMaxDecel : kMaxAccel; + float accel_clamp = math_util::Clamp(velocity, -kMaxSpeed, kMaxSpeed); + return math_util::Clamp(accel_clamp, lastSpeed_ - max_decel * kCommandInterval, + lastSpeed_ + max_accel * kCommandInterval); +} + +float RadioDriver::clampCurvature(float curvature) const { + return math_util::Clamp(curvature, -kMaxCurvature, kMaxCurvature); } } // namespace radio_driver diff --git a/src/radio_driver/radio_driver.h b/src/radio_driver/radio_driver.h index 83e6308..62d87c8 100644 --- a/src/radio_driver/radio_driver.h +++ b/src/radio_driver/radio_driver.h @@ -1,6 +1,9 @@ +#include #include #include +#include "radio_driver/radio_interface.h" + namespace radio_driver { class RadioDriver { @@ -8,11 +11,42 @@ class RadioDriver { RadioDriver(ros::NodeHandle nh); private: - void joystickCallback(const sensor_msgs::Joy& msg); + enum class DriveMode { + Disabled, + Manual, + Autonomous, + ContinuousAutonomous, + }; + + ros::Publisher odom_pub_; + ros::Publisher drive_pub_; + ros::Publisher autonomy_enabler_pub_; + + ros::SteadyTimer timer_; void timerCallback(const ros::SteadyTimerEvent& event); + ros::Subscriber ackermann_curvature_sub_; + void ackermannCurvatureCallback(const amrl_msgs::AckermannCurvatureDriveMsg& msg); + + float ackermann_velocity_; + float ackermann_curvature_; + ros::Subscriber joystick_sub_; - ros::SteadyTimer timer_; + void joystickCallback(const sensor_msgs::Joy& msg); + + float joystick_velocity_; + float joystick_curvature_; + + float clampVelocity(float velocity) const; + float clampCurvature(float curvature) const; + + DriveMode drive_mode_; + RadioInterface radio_interface_; + + float lastSpeed_; + float lastCurvature_; + float lastThrottle_; + float lastSteering_; }; -} // namespace radio_driver \ No newline at end of file +} // namespace radio_driver diff --git a/src/radio_driver/radio_interface.cpp b/src/radio_driver/radio_interface.cpp new file mode 100644 index 0000000..325d309 --- /dev/null +++ b/src/radio_driver/radio_interface.cpp @@ -0,0 +1,63 @@ +#include "radio_driver/radio_interface.h" + +#include + +namespace radio_driver { + +RadioInterface::RadioInterface() : connected_(false) { + buffer_ = new char[12]; // TODO: check if 12 is enough +} + +RadioInterface::~RadioInterface() { + if (!disconnect()) { + LOG(ERROR) << "Failed to cleanly disconnect from radio on shutdown"; + } + + delete[] buffer_; +} + +bool RadioInterface::isConnected() const { return connected_; } + +bool RadioInterface::sendControl(float throttle, float steering) { + if (!connected_) { + LOG(ERROR) << "Attempted to send control to radio while disconnected"; + return false; + } + + commandToBuffer(throttle, steering); + // TODO: send buf to serial device + return true; +} + +bool RadioInterface::connect(const std::string& port) { + // TODO: actually connect to serial device + connected_ = true; + return true; +} + +bool RadioInterface::disconnect() { + // TODO: actually disconnect from serial device + connected_ = false; + return true; +} + +void RadioInterface::commandToBuffer(float throttle, float steering) { + static float min_val = 1000; + static float max_val = 2000; + + // TODO: print these to see if they are correct + uint32_t throttle_pwm = (throttle + 1) * (max_val - min_val) / 2 + min_val; + uint32_t steering_pwm = (steering + 1) * (max_val - min_val) / 2 + min_val; + + if (throttle_pwm < min_val || throttle_pwm > max_val) { + LOG(ERROR) << "Throttle command out of range: " << throttle_pwm; + } else if (steering_pwm < min_val || steering_pwm > max_val) { + LOG(ERROR) << "Steering command out of range: " << steering_pwm; + } else { + std::ostringstream ss; + ss << throttle_pwm << "," << steering_pwm << "\n"; + ss.str().copy(buffer_, ss.str().size()); + } +} + +} // namespace radio_driver diff --git a/src/radio_driver/radio_interface.h b/src/radio_driver/radio_interface.h new file mode 100644 index 0000000..4a7ac99 --- /dev/null +++ b/src/radio_driver/radio_interface.h @@ -0,0 +1,29 @@ +#include + +namespace radio_driver { + +class RadioInterface { + public: + RadioInterface(); + + ~RadioInterface(); + + bool isConnected() const; + + // send a command to the car via the radio + // throttle and steering are in the range [-1, 1] + bool sendControl(float throttle, float steering); + + private: + bool connect(const std::string& port); + bool disconnect(); + + // convert throttle and steering commands to ascii bytes in the format arduino expects + // throttle and steering are in the range [-1, 1] + void commandToBuffer(float throttle, float steering); + + char* buffer_; + bool connected_; +}; + +} // namespace radio_driver \ No newline at end of file