diff --git a/.clang-format b/.clang-format index d83d6e4..d86af92 100644 --- a/.clang-format +++ b/.clang-format @@ -92,10 +92,6 @@ PenaltyBreakString: 1000 PenaltyExcessCharacter: 1000000 PenaltyReturnTypeOnItsOwnLine: 200 PointerAlignment: Left -RawStringFormats: - - Delimiter: pb - Language: TextProto - BasedOnStyle: google ReflowComments: true SortIncludes: true SortUsingDeclarations: true diff --git a/README.md b/README.md index ddc44dc..de804f2 100644 --- a/README.md +++ b/README.md @@ -18,29 +18,6 @@ Pulse duration and period (in nanoseconds) of servo X. The values are passed through `data` array. Publishing to pwm topic overrides angle value and vice versa. -* **`core2/reset_board`** ([std_msgs/Empty]) - - Performs software reset on the CORE2 board. - -* **`core2/reset_config`** ([std_msgs/Empty]) - - Loads the default config and saves it to persistant storage. - -* **`core2/set_imu`** ([std_msgs/Bool]) - - Enables or disables the IMU sensor and saves the configuration to persistent storage. - Requires a reset to apply. - -* **`core2/set_gps`** ([std_msgs/Bool]) - - Enables or disables the GPS sensor and saves the configuration to persistent storage. - Requires a reset to apply. - -* **`core2/set_debug`** ([std_msgs/Bool]) - - Enables or disables debug messages. - For the messages to be sent to rosout, you also need to set the logger level of rosserial node to Debug. When enabled, it can cause issues with the rosserial communication due to high throughput. - ### Published topics * **`wheel_odom`** ([geometry_msgs/TwistStamped]) @@ -76,6 +53,33 @@ ### Services +* **`core2/reset_board`** ([std_srvs/Empty]) + + Performs software reset on the CORE2 board. + +* **`core2/reset_config`** ([std_srvs/Trigger]) + + Loads the default config and saves it to persistant storage. + +* **`core2/get_firmware_version`** ([std_srvs/Trigger]) + + Performs software reset on the CORE2 board. + +* **`core2/set_imu`** ([std_srvs/SetBool]) + + Enables or disables the IMU sensor and saves the configuration to persistent storage. + Requires a reset to apply. + +* **`core2/set_gps`** ([std_srvs/SetBool]) + + Enables or disables the GPS sensor and saves the configuration to persistent storage. + Requires a reset to apply. + +* **`core2/set_debug`** ([std_srvs/SetBool]) + + Enables or disables debug messages. + For the messages to be sent to rosout, you also need to set the logger level of rosserial node to Debug. When enabled, it can cause issues with the rosserial communication due to high throughput. + * **`imu/calibrate_gyro_accel`** ([std_srvs/Trigger]) (**only if IMU is enabled**) Calibrates gyroscope and accelerometer biases and stores them in persistent storage. @@ -182,8 +186,9 @@ [std_msgs/Int16]: http://docs.ros.org/api/std_msgs/html/msg/Int16.html [std_msgs/Float32]: http://docs.ros.org/api/std_msgs/html/msg/Float32.html [std_msgs/UInt16MultiArray]: http://docs.ros.org/api/std_msgs/html/msg/UInt16MultiArray.html -[std_msgs/Bool]: http://docs.ros.org/api/std_msgs/html/msg/Bool.html -[std_msgs/Empty]: http://docs.ros.org/api/std_msgs/html/msg/Empty.html +[std_srvs/Empty]: http://docs.ros.org/api/std_srvs/html/srv/Empty.html +[std_srvs/Trigger]: http://docs.ros.org/api/std_srvs/html/srv/Trigger.html +[std_srvs/SetBool]: http://docs.ros.org/api/std_srvs/html/srv/SetBool.html [sensor_msgs/JointState]: http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html [geometry_msgs/Vector3Stamped]: http://docs.ros.org/api/geometry_msgs/html/msg/Vector3Stamped.html [std_srvs/Trigger]: http://docs.ros.org/api/std_srvs/html/srv/Trigger.html diff --git a/include/leo_firmware/utils.h b/include/leo_firmware/utils.h index ba26fd7..6ebeae3 100644 --- a/include/leo_firmware/utils.h +++ b/include/leo_firmware/utils.h @@ -56,7 +56,7 @@ class ServoWrapper { int width_min = 1000; int width_max = 2000; - std::string param_prefix = "core2/servo" + std::to_string(num_) + "/"; + std::string param_prefix = std::string("core2/servo") + static_cast(num_ + '0') + '/'; nh->getParam((param_prefix + "period").c_str(), &servo_period); nh->getParam((param_prefix + "angle_min").c_str(), &angle_min); nh->getParam((param_prefix + "angle_max").c_str(), &angle_max); diff --git a/include/leo_firmware/wheel_controller.h b/include/leo_firmware/wheel_controller.h index 53639c4..92dd2e4 100644 --- a/include/leo_firmware/wheel_controller.h +++ b/include/leo_firmware/wheel_controller.h @@ -9,6 +9,8 @@ #include +#define WHEEL_VELOCITY_REJECTION_THRESHOLD 2.0 + class WheelController { public: WheelController(hMotor& motor, const bool polarity, const float max_speed, @@ -40,6 +42,7 @@ class WheelController { bool turned_on_; int32_t ticks_now_; + int32_t ticks_offset_; int32_t ticks_sum_; uint32_t dt_sum_; diff --git a/main.cpp b/main.cpp index d3ec88b..2d530aa 100644 --- a/main.cpp +++ b/main.cpp @@ -6,11 +6,11 @@ #include #include #include -#include -#include #include #include #include +#include +#include #include #include @@ -52,19 +52,6 @@ sensor_msgs::NavSatFix gps_fix; ros::Publisher *gps_pub; bool publish_gps = false; -ros::Subscriber *twist_sub; - -ros::Subscriber *reset_board_sub; -ros::Subscriber *reset_config_sub; -ros::Subscriber *set_imu_sub; -ros::Subscriber *set_gps_sub; -ros::Subscriber *set_debug_sub; - -ros::ServiceServer - *imu_cal_mpu_srv; -ros::ServiceServer - *imu_cal_mag_srv; - DiffDriveController dc; ServoWrapper servo1(1, hServo.servo1); @@ -80,32 +67,48 @@ void cmdVelCallback(const geometry_msgs::Twist &msg) { dc.setSpeed(msg.linear.x, msg.angular.z); } -void resetBoardCallback(const std_msgs::Empty &msg) { +void resetBoardCallback(const std_srvs::EmptyRequest &req, + std_srvs::EmptyResponse &res) { logDebug("[resetBoardCallback]"); sys.reset(); } -void resetConfigCallback(const std_msgs::Empty &msg) { +void resetConfigCallback(const std_srvs::TriggerRequest &req, + std_srvs::TriggerResponse &res) { logDebug("[resetConfigCallback]"); reset_config(); + res.success = true; } -void setImuCallback(const std_msgs::Bool &msg) { - logDebug("[setImuCallback] %s", msg.data ? "true" : "false"); - conf.imu_enabled = msg.data; +void getFirmwareCallback(const std_srvs::TriggerRequest &req, + std_srvs::TriggerResponse &res) { + logDebug("[getFirmwareCallback]"); + res.message = FIRMWARE_VERSION; + res.success = true; +} + +void setImuCallback(const std_srvs::SetBoolRequest &req, + std_srvs::SetBoolResponse &res) { + logDebug("[setImuCallback] %s", req.data ? "true" : "false"); + conf.imu_enabled = req.data; store_config(); + res.success = true; } -void setGpsCallback(const std_msgs::Bool &msg) { - logDebug("[setGpsCallback] %s", msg.data ? "true" : "false"); - conf.gps_enabled = msg.data; +void setGpsCallback(const std_srvs::SetBoolRequest &req, + std_srvs::SetBoolResponse &res) { + logDebug("[setGpsCallback] %s", req.data ? "true" : "false"); + conf.gps_enabled = req.data; store_config(); + res.success = true; } -void setDebugCallback(const std_msgs::Bool &msg) { - logDebug("[setDebugCallback] %s", msg.data ? "true" : "false"); - conf.debug_logging = msg.data; +void setDebugCallback(const std_srvs::SetBoolRequest &req, + std_srvs::SetBoolResponse &res) { + logDebug("[setDebugCallback] %s", req.data ? "true" : "false"); + conf.debug_logging = req.data; store_config(); + res.success = true; } void calMpuCallback(const std_srvs::TriggerRequest &req, @@ -125,71 +128,52 @@ void calMagCallback(const std_srvs::TriggerRequest &req, } void initROS() { + // Publishers battery_pub = new ros::Publisher("battery", &battery); odom_pub = new ros::Publisher("wheel_odom", &odom); joint_states_pub = new ros::Publisher("joint_states", &joint_states); - twist_sub = + nh.advertise(*battery_pub); + nh.advertise(*odom_pub); + nh.advertise(*joint_states_pub); + + // Subscribers + auto twist_sub = new ros::Subscriber("cmd_vel", &cmdVelCallback); - reset_board_sub = new ros::Subscriber("core2/reset_board", - &resetBoardCallback); - reset_config_sub = new ros::Subscriber("core2/reset_config", - &resetConfigCallback); - set_imu_sub = - new ros::Subscriber("core2/set_imu", &setImuCallback); - set_gps_sub = - new ros::Subscriber("core2/set_gps", &setGpsCallback); - set_debug_sub = - new ros::Subscriber("core2/set_debug", &setDebugCallback); - - ros::Subscriber *servo1_angle_sub = - new ros::Subscriber( - "servo1/angle", &ServoWrapper::angleCallback, &servo1); - ros::Subscriber *servo2_angle_sub = - new ros::Subscriber( - "servo2/angle", &ServoWrapper::angleCallback, &servo2); - ros::Subscriber *servo3_angle_sub = - new ros::Subscriber( - "servo3/angle", &ServoWrapper::angleCallback, &servo3); - ros::Subscriber *servo4_angle_sub = - new ros::Subscriber( - "servo4/angle", &ServoWrapper::angleCallback, &servo4); - ros::Subscriber *servo5_angle_sub = - new ros::Subscriber( - "servo5/angle", &ServoWrapper::angleCallback, &servo5); - ros::Subscriber *servo6_angle_sub = - new ros::Subscriber( - "servo6/angle", &ServoWrapper::angleCallback, &servo6); - - ros::Subscriber *servo1_pwm_sub = + auto servo1_angle_sub = new ros::Subscriber( + "servo1/angle", &ServoWrapper::angleCallback, &servo1); + auto servo2_angle_sub = new ros::Subscriber( + "servo2/angle", &ServoWrapper::angleCallback, &servo2); + auto servo3_angle_sub = new ros::Subscriber( + "servo3/angle", &ServoWrapper::angleCallback, &servo3); + auto servo4_angle_sub = new ros::Subscriber( + "servo4/angle", &ServoWrapper::angleCallback, &servo4); + auto servo5_angle_sub = new ros::Subscriber( + "servo5/angle", &ServoWrapper::angleCallback, &servo5); + auto servo6_angle_sub = new ros::Subscriber( + "servo6/angle", &ServoWrapper::angleCallback, &servo6); + + auto servo1_pwm_sub = new ros::Subscriber( "servo1/pwm", &ServoWrapper::pwmCallback, &servo1); - ros::Subscriber *servo2_pwm_sub = + auto servo2_pwm_sub = new ros::Subscriber( "servo2/pwm", &ServoWrapper::pwmCallback, &servo2); - ros::Subscriber *servo3_pwm_sub = + auto servo3_pwm_sub = new ros::Subscriber( "servo3/pwm", &ServoWrapper::pwmCallback, &servo3); - ros::Subscriber *servo4_pwm_sub = + auto servo4_pwm_sub = new ros::Subscriber( "servo4/pwm", &ServoWrapper::pwmCallback, &servo4); - ros::Subscriber *servo5_pwm_sub = + auto servo5_pwm_sub = new ros::Subscriber( "servo5/pwm", &ServoWrapper::pwmCallback, &servo5); - ros::Subscriber *servo6_pwm_sub = + auto servo6_pwm_sub = new ros::Subscriber( "servo6/pwm", &ServoWrapper::pwmCallback, &servo6); - nh.advertise(*battery_pub); - nh.advertise(*odom_pub); - nh.advertise(*joint_states_pub); nh.subscribe(*twist_sub); - nh.subscribe(*reset_board_sub); - nh.subscribe(*reset_config_sub); - nh.subscribe(*set_imu_sub); - nh.subscribe(*set_gps_sub); - nh.subscribe(*set_debug_sub); nh.subscribe(*servo1_angle_sub); nh.subscribe(*servo2_angle_sub); nh.subscribe(*servo3_angle_sub); @@ -203,15 +187,49 @@ void initROS() { nh.subscribe(*servo5_pwm_sub); nh.subscribe(*servo6_pwm_sub); + // Services + auto reset_board_srv = + new ros::ServiceServer( + "core2/reset_board", &resetBoardCallback); + auto reset_config_srv = new ros::ServiceServer( + "core2/reset_config", &resetConfigCallback); + auto firmware_version_srv = new ros::ServiceServer( + "core2/get_firmware_version", &getFirmwareCallback); + auto set_imu_srv = new ros::ServiceServer( + "core2/set_imu", &setImuCallback); + auto set_gps_srv = new ros::ServiceServer( + "core2/set_gps", &setGpsCallback); + auto set_debug_srv = new ros::ServiceServer( + "core2/set_debug", &setDebugCallback); + + nh.advertiseService( + *reset_board_srv); + nh.advertiseService( + *reset_config_srv); + nh.advertiseService( + *firmware_version_srv); + nh.advertiseService( + *set_imu_srv); + nh.advertiseService( + *set_gps_srv); + nh.advertiseService( + *set_debug_srv); + + // IMU if (conf.imu_enabled) { imu_gyro_pub = new ros::Publisher("imu/gyro", &imu_gyro_msg); imu_accel_pub = new ros::Publisher("imu/accel", &imu_accel_msg); imu_mag_pub = new ros::Publisher("imu/mag", &imu_mag_msg); - imu_cal_mpu_srv = new ros::ServiceServer( + auto imu_cal_mpu_srv = new ros::ServiceServer( "imu/calibrate_gyro_accel", &calMpuCallback); - imu_cal_mag_srv = new ros::ServiceServer( + auto imu_cal_mag_srv = new ros::ServiceServer( "imu/calibrate_mag", &calMagCallback); nh.advertise(*imu_gyro_pub); nh.advertise(*imu_accel_pub); @@ -222,6 +240,7 @@ void initROS() { *imu_cal_mag_srv); } + // GPS if (conf.gps_enabled) { gps_pub = new ros::Publisher("gps_fix", &gps_fix); nh.advertise(*gps_pub); diff --git a/params.h b/params.h index 1b5c360..f6ac86d 100644 --- a/params.h +++ b/params.h @@ -3,6 +3,8 @@ #include "hFramework.h" +static const char *FIRMWARE_VERSION = "1.1.0"; + // The hSens port to which the IMU is connected // Set to either hSens1 or hSens2 static hFramework::hSensor_i2c &IMU_HSENS = hSens2; diff --git a/src/diff_drive_controller.cpp b/src/diff_drive_controller.cpp index 263b94b..e8d5480 100644 --- a/src/diff_drive_controller.cpp +++ b/src/diff_drive_controller.cpp @@ -52,7 +52,6 @@ void DiffDriveController::start() { last_update_ = sys.getRefTime(); sys.taskCreate(std::bind(&DiffDriveController::inputWatchdog, this)); } - sys.taskCreate(std::bind(&DiffDriveController::debugLoop, this)); } void DiffDriveController::setSpeed(float linear, float angular) { @@ -150,21 +149,6 @@ void DiffDriveController::updateOdometryLoop() { } } -void DiffDriveController::debugLoop() { - uint32_t t = sys.getRefTime(); - uint32_t dt = 100; - - while (true) { - logDebug("[Motor power] FL: %d RL: %d FR: %d RR: %d", wheel_FL_->getPower(), - wheel_RL_->getPower(), wheel_FR_->getPower(), - wheel_RR_->getPower()); - logDebug("[Motor speed] FL: %f RL: %f FR: %f RR: %f", wheel_FL_->getSpeed(), - wheel_RL_->getSpeed(), wheel_FR_->getSpeed(), - wheel_RR_->getSpeed()); - sys.delaySync(t, dt); - } -} - void DiffDriveController::inputWatchdog() { while (true) { while (sys.getRefTime() < last_update_ + input_timeout_) diff --git a/src/wheel_controller.cpp b/src/wheel_controller.cpp index 63fda35..4795e73 100644 --- a/src/wheel_controller.cpp +++ b/src/wheel_controller.cpp @@ -14,6 +14,7 @@ WheelController::WheelController(hMotor &motor, const bool polarity, torque_limit_(torque_limit), turned_on_(true), ticks_now_(0), + ticks_offset_(0), ticks_sum_(0), dt_sum_(0), v_now_(0.0), @@ -40,10 +41,17 @@ WheelController::WheelController(hMotor &motor, const bool polarity, void WheelController::update(uint32_t dt) { int32_t ticks_prev = ticks_now_; - ticks_now_ = motor_.getEncoderCnt(); + ticks_now_ = motor_.getEncoderCnt() - ticks_offset_; int32_t new_ticks = ticks_now_ - ticks_prev; + float ins_vel = static_cast(std::abs(new_ticks)) / (dt * 0.001); + if (ins_vel > WHEEL_VELOCITY_REJECTION_THRESHOLD * max_speed_) { + ticks_offset_ += new_ticks; + ticks_now_ -= new_ticks; + new_ticks = 0; + } + std::pair encoder_old = encoder_buffer_.push_back(std::pair(new_ticks, dt)); @@ -92,6 +100,8 @@ void WheelController::reset() { v_now_ = 0; v_target_ = 0; motor_.setPower(0); + ticks_now_ = 0; + ticks_offset_ = 0; } void WheelController::turnOff() {