progress except for serial connection

.clang-format

@@ -0,0 +1,5 @@
+BasedOnStyle: Google
+DerivePointerAlignment: false
+PointerAlignment: Left
+IndentWidth: 4
+ColumnLimit: 100

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()
 

src/radio_driver/radio_driver.cpp

@@ -1,26 +1,140 @@
 #include "radio_driver/radio_driver.h"
 
+#include <geometry_msgs/TwistStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <std_msgs/Bool.h>
+
+#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<nav_msgs::Odometry>("odom", 1);
+    drive_pub_ = nh.advertise<geometry_msgs::TwistStamped>("radio_drive", 1);
+    autonomy_enabler_pub_ = nh.advertise<std_msgs::Bool>("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

src/radio_driver/radio_driver.h

@@ -1,18 +1,52 @@
+#include <amrl_msgs/AckermannCurvatureDriveMsg.h>
 #include <ros/ros.h>
 #include <sensor_msgs/Joy.h>
 
+#include "radio_driver/radio_interface.h"
+
 namespace radio_driver {
 
 class RadioDriver {
    public:
     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
+}  // namespace radio_driver

src/radio_driver/radio_interface.cpp

@@ -0,0 +1,63 @@
+#include "radio_driver/radio_interface.h"
+
+#include <glog/logging.h>
+
+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

src/radio_driver/radio_interface.h

@@ -0,0 +1,29 @@
+#include <string>
+
+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