From 25623760a89bbf1e9263e99e15c1c349b0368dac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Tue, 14 Apr 2020 16:11:06 +0200 Subject: [PATCH 01/10] Replace to_string function with stringstream --- include/leo_firmware/utils.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/leo_firmware/utils.h b/include/leo_firmware/utils.h index ba26fd7..17e95e6 100644 --- a/include/leo_firmware/utils.h +++ b/include/leo_firmware/utils.h @@ -2,6 +2,7 @@ #define LEO_FIRMWARE_INCLUDE_UTILS_H_ #include +#include #include @@ -56,7 +57,9 @@ class ServoWrapper { int width_min = 1000; int width_max = 2000; - std::string param_prefix = "core2/servo" + std::to_string(num_) + "/"; + std::ostringstream ss; + ss << "core2/servo" << num_ << "/"; + std::string param_prefix = ss.str(); 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); From 8d6dddbaa193cd65efe7121ceb74419333539637 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 23 Jul 2020 17:17:28 +0200 Subject: [PATCH 02/10] Use static casting instead of stringstreams as sstream library adds too much size overhead --- include/leo_firmware/utils.h | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/include/leo_firmware/utils.h b/include/leo_firmware/utils.h index 17e95e6..6ebeae3 100644 --- a/include/leo_firmware/utils.h +++ b/include/leo_firmware/utils.h @@ -2,7 +2,6 @@ #define LEO_FIRMWARE_INCLUDE_UTILS_H_ #include -#include #include @@ -57,9 +56,7 @@ class ServoWrapper { int width_min = 1000; int width_max = 2000; - std::ostringstream ss; - ss << "core2/servo" << num_ << "/"; - std::string param_prefix = ss.str(); + 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); From e82931786b952f5948b8f014bfbc3d4e235ff68c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 16 Jul 2020 19:21:45 +0200 Subject: [PATCH 03/10] Fix the issue with encoder readings by detecting velocity spikes --- include/leo_firmware/wheel_controller.h | 3 +++ src/wheel_controller.cpp | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) 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/src/wheel_controller.cpp b/src/wheel_controller.cpp index 63fda35..84e84f3 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)); From c52f411d35839268f8e28c6db02f0fa3b8084d1a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 16 Jul 2020 19:32:35 +0200 Subject: [PATCH 04/10] Add resetting ticks variables to reset function --- src/wheel_controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/wheel_controller.cpp b/src/wheel_controller.cpp index 84e84f3..4795e73 100644 --- a/src/wheel_controller.cpp +++ b/src/wheel_controller.cpp @@ -100,6 +100,8 @@ void WheelController::reset() { v_now_ = 0; v_target_ = 0; motor_.setPower(0); + ticks_now_ = 0; + ticks_offset_ = 0; } void WheelController::turnOff() { From 623693ae6e830f9ad6c0ffea13e2206aca0b0037 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 30 Jul 2020 17:57:45 +0200 Subject: [PATCH 05/10] Modify .clang-format file to work with clang-format 10 --- .clang-format | 4 ---- 1 file changed, 4 deletions(-) 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 From 38e5e4d3527c17174f809127e2fbfb0adc658cf3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 30 Jul 2020 17:58:34 +0200 Subject: [PATCH 06/10] Remove debug loop from diff drive controller --- src/diff_drive_controller.cpp | 16 ---------------- 1 file changed, 16 deletions(-) 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_) From 0a49531e3f32f1a2b9e7462d7ab6db6a342504a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 30 Jul 2020 18:01:37 +0200 Subject: [PATCH 07/10] Add service for getting the current firmware version --- main.cpp | 94 ++++++++++++++++++++++++++------------------------------ params.h | 2 ++ 2 files changed, 46 insertions(+), 50 deletions(-) diff --git a/main.cpp b/main.cpp index d3ec88b..47bec2e 100644 --- a/main.cpp +++ b/main.cpp @@ -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); @@ -108,6 +95,13 @@ void setDebugCallback(const std_msgs::Bool &msg) { store_config(); } +void getFirmwareCallback(const std_srvs::TriggerRequest &req, + std_srvs::TriggerResponse &res) { + logDebug("[getFirmwareCallback]"); + res.message = FIRMWARE_VERSION; + res.success = true; +} + void calMpuCallback(const std_srvs::TriggerRequest &req, std_srvs::TriggerResponse &res) { logDebug("[calMpuCallback]"); @@ -129,58 +123,56 @@ void initROS() { odom_pub = new ros::Publisher("wheel_odom", &odom); joint_states_pub = new ros::Publisher("joint_states", &joint_states); - twist_sub = + 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 = + auto reset_board_sub = new ros::Subscriber( + "core2/reset_board", &resetBoardCallback); + auto reset_config_sub = new ros::Subscriber( + "core2/reset_config", &resetConfigCallback); + auto set_imu_sub = new ros::Subscriber("core2/set_imu", &setImuCallback); - set_gps_sub = + auto set_gps_sub = new ros::Subscriber("core2/set_gps", &setGpsCallback); - set_debug_sub = + auto 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); + auto firmware_version_srv = new ros::ServiceServer( + "core2/get_firmware_version", &getFirmwareCallback); + nh.advertise(*battery_pub); nh.advertise(*odom_pub); nh.advertise(*joint_states_pub); @@ -202,16 +194,18 @@ void initROS() { nh.subscribe(*servo4_pwm_sub); nh.subscribe(*servo5_pwm_sub); nh.subscribe(*servo6_pwm_sub); + nh.advertiseService( + *firmware_version_srv); 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); 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; From 741f0d23ee6a92baee960b7804da1adfbf8c8c32 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 30 Jul 2020 18:37:56 +0200 Subject: [PATCH 08/10] Use services instead of topics for reset triggers and setting config options --- main.cpp | 108 +++++++++++++++++++++++++++++++++---------------------- 1 file changed, 65 insertions(+), 43 deletions(-) diff --git a/main.cpp b/main.cpp index 47bec2e..3d240d3 100644 --- a/main.cpp +++ b/main.cpp @@ -6,11 +6,10 @@ #include #include #include -#include -#include #include #include #include +#include #include #include @@ -67,38 +66,47 @@ void cmdVelCallback(const geometry_msgs::Twist &msg) { dc.setSpeed(msg.linear.x, msg.angular.z); } -void resetBoardCallback(const std_msgs::Empty &msg) { +void getFirmwareCallback(const std_srvs::TriggerRequest &req, + std_srvs::TriggerResponse &res) { + logDebug("[getFirmwareCallback]"); + res.message = FIRMWARE_VERSION; + res.success = true; +} + +void resetBoardCallback(const std_srvs::TriggerRequest &req, + std_srvs::TriggerResponse &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 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(); -} - -void getFirmwareCallback(const std_srvs::TriggerRequest &req, - std_srvs::TriggerResponse &res) { - logDebug("[getFirmwareCallback]"); - res.message = FIRMWARE_VERSION; res.success = true; } @@ -119,24 +127,19 @@ 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); + nh.advertise(*battery_pub); + nh.advertise(*odom_pub); + nh.advertise(*joint_states_pub); + + // Subscribers auto twist_sub = new ros::Subscriber("cmd_vel", &cmdVelCallback); - auto reset_board_sub = new ros::Subscriber( - "core2/reset_board", &resetBoardCallback); - auto reset_config_sub = new ros::Subscriber( - "core2/reset_config", &resetConfigCallback); - auto set_imu_sub = - new ros::Subscriber("core2/set_imu", &setImuCallback); - auto set_gps_sub = - new ros::Subscriber("core2/set_gps", &setGpsCallback); - auto set_debug_sub = - new ros::Subscriber("core2/set_debug", &setDebugCallback); - auto servo1_angle_sub = new ros::Subscriber( "servo1/angle", &ServoWrapper::angleCallback, &servo1); auto servo2_angle_sub = new ros::Subscriber( @@ -169,19 +172,7 @@ void initROS() { new ros::Subscriber( "servo6/pwm", &ServoWrapper::pwmCallback, &servo6); - auto firmware_version_srv = new ros::ServiceServer( - "core2/get_firmware_version", &getFirmwareCallback); - - 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); @@ -194,8 +185,39 @@ void initROS() { nh.subscribe(*servo4_pwm_sub); nh.subscribe(*servo5_pwm_sub); nh.subscribe(*servo6_pwm_sub); + + // Services + auto firmware_version_srv = new ros::ServiceServer( + "core2/get_firmware_version", &getFirmwareCallback); + auto reset_board_srv = new ros::ServiceServer( + "core2/reset_board", &resetBoardCallback); + auto reset_config_srv = new ros::ServiceServer( + "core2/reset_config", &resetConfigCallback); + 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( + *firmware_version_srv); + nh.advertiseService( + *reset_board_srv); nh.advertiseService( - *firmware_version_srv); + *reset_config_srv); + nh.advertiseService( + *set_imu_srv); + nh.advertiseService( + *set_gps_srv); + nh.advertiseService( + *set_debug_srv); if (conf.imu_enabled) { imu_gyro_pub = new ros::Publisher("imu/gyro", &imu_gyro_msg); From e17b31856383b21da73c36cc0f29a6faeb7c05f6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 30 Jul 2020 18:48:10 +0200 Subject: [PATCH 09/10] Change reset_board service type to Empty --- main.cpp | 39 +++++++++++++++++++++------------------ 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/main.cpp b/main.cpp index 3d240d3..2d530aa 100644 --- a/main.cpp +++ b/main.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -66,15 +67,8 @@ void cmdVelCallback(const geometry_msgs::Twist &msg) { dc.setSpeed(msg.linear.x, msg.angular.z); } -void getFirmwareCallback(const std_srvs::TriggerRequest &req, - std_srvs::TriggerResponse &res) { - logDebug("[getFirmwareCallback]"); - res.message = FIRMWARE_VERSION; - res.success = true; -} - -void resetBoardCallback(const std_srvs::TriggerRequest &req, - std_srvs::TriggerResponse &res) { +void resetBoardCallback(const std_srvs::EmptyRequest &req, + std_srvs::EmptyResponse &res) { logDebug("[resetBoardCallback]"); sys.reset(); } @@ -86,6 +80,13 @@ void resetConfigCallback(const std_srvs::TriggerRequest &req, res.success = true; } +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"); @@ -187,15 +188,15 @@ void initROS() { nh.subscribe(*servo6_pwm_sub); // Services - auto firmware_version_srv = new ros::ServiceServer( - "core2/get_firmware_version", &getFirmwareCallback); - auto reset_board_srv = new ros::ServiceServer( - "core2/reset_board", &resetBoardCallback); + 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); @@ -206,12 +207,12 @@ void initROS() { std_srvs::SetBoolResponse>( "core2/set_debug", &setDebugCallback); - nh.advertiseService( - *firmware_version_srv); - nh.advertiseService( + nh.advertiseService( *reset_board_srv); nh.advertiseService( *reset_config_srv); + nh.advertiseService( + *firmware_version_srv); nh.advertiseService( *set_imu_srv); nh.advertiseService( @@ -219,6 +220,7 @@ void initROS() { 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); @@ -238,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); From 0d32bef739a427f839359336ea5d9e2d3031f85f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C5=82a=C5=BCej=20Sowa?= Date: Thu, 30 Jul 2020 18:52:16 +0200 Subject: [PATCH 10/10] Update README --- README.md | 55 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 30 insertions(+), 25 deletions(-) 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