From 596c15fe01543eb08e77bc807384276c918031b1 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 20 Feb 2018 17:35:05 -0500 Subject: [PATCH 001/200] merge navigation/dev into navx/dev --- external/cppfs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/cppfs b/external/cppfs index f9c8e6f..997f6a8 160000 --- a/external/cppfs +++ b/external/cppfs @@ -1 +1 @@ -Subproject commit f9c8e6f7d1dd1473eda4dd93a5be680568de66e0 +Subproject commit 997f6a8c5d1622df05251a4164986c3a7954dc0e From 0493e1391f4f87a7591f04dfd472ee0f0690299a Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 20 Feb 2018 17:39:22 -0500 Subject: [PATCH 002/200] cppfs change --- external/cppfs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/cppfs b/external/cppfs index 997f6a8..f9c8e6f 160000 --- a/external/cppfs +++ b/external/cppfs @@ -1 +1 @@ -Subproject commit 997f6a8c5d1622df05251a4164986c3a7954dc0e +Subproject commit f9c8e6f7d1dd1473eda4dd93a5be680568de66e0 From c633b937d0e0d0035bcadc12d32b696027edc657 Mon Sep 17 00:00:00 2001 From: solsane Date: Thu, 22 Feb 2018 09:23:44 -0500 Subject: [PATCH 003/200] [some large lines] -> [more],[small],[lines] --- .../navigation_actions/drive_straight.hpp | 6 ++++-- .../navigation/actions/src/drive_straight.cpp | 6 ++++-- core/navigation/actions/src/turn_to_angle.cpp | 20 ++++++++++++++++++- .../include/drivetrains/drivetrain.hpp | 6 ++++-- .../drivetrains/two_roboclaw_drivetrain.hpp | 9 ++++++--- .../src/two_roboclaw_drivetrain.cpp | 5 ++++- 6 files changed, 41 insertions(+), 11 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_straight.hpp b/core/navigation/actions/include/navigation_actions/drive_straight.hpp index 7bbd698..db38c71 100644 --- a/core/navigation/actions/include/navigation_actions/drive_straight.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_straight.hpp @@ -18,9 +18,11 @@ namespace rip class DriveStraight : public framework::Action { public: - DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Distance& distance, double p, double i, double d); + DriveStraight(const std::string& name, std::shared_ptr drivetrain, + const units::Distance& distance, double p, double i, double d); - DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Time& time, const units::Velocity& speed); + DriveStraight(const std::string& name, std::shared_ptr drivetrain, + const units::Time& time, const units::Velocity& speed); /** * Returns whether or not the action has finished execution. diff --git a/core/navigation/actions/src/drive_straight.cpp b/core/navigation/actions/src/drive_straight.cpp index 23ce6e8..2b3c09d 100644 --- a/core/navigation/actions/src/drive_straight.cpp +++ b/core/navigation/actions/src/drive_straight.cpp @@ -6,14 +6,16 @@ namespace rip { namespace actions { - DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Distance& distance, double p, double i, double d) + DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, + const units::Distance& distance, double p, double i, double d) : Action(name) , m_use_time(false) , m_distance(distance) , m_drivetrain(drivetrain) {} - DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, const units::Time& time, const units::Velocity& speed) + DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, + const units::Time& time, const units::Velocity& speed) : Action(name) , m_use_time(true) , m_time(time) diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 0577d9a..4e9eb39 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -6,7 +6,25 @@ namespace rip { namespace actions { + TurnToAngle::isFinished() + { + } + + TurnToAngle::update() + { + + } + + TurnToAngle::setup() + { + + } + + TurnToAngle::teardown() + { + + } } } -} \ No newline at end of file +} diff --git a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp index d03f7dc..fd81645 100644 --- a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp +++ b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp @@ -39,7 +39,8 @@ namespace rip * * all range from [-1, 1] */ - virtual void drive(double front_left, double front_right, double back_left, double back_rightk) = 0; + virtual void drive(double front_left, double front_right, double back_left, + double back_rightk) = 0; /** * Single command to all motors @@ -54,7 +55,8 @@ namespace rip /** * Command four wheels separately */ - virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) = 0; + virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, + const MotorDynamics& back_left, const MotorDynamics& back_right) = 0; }; } } diff --git a/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp index a731f1f..e2cc4db 100644 --- a/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp +++ b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp @@ -23,7 +23,8 @@ namespace rip using NavX = navx::NavX; using MotorDynamics = motorcontrollers::MotorDynamics; public: - TwoRoboclawDrivetrain(const std::string& name, std::shared_ptr left, std::shared_ptr right, std::shared_ptr navx = nullptr); + TwoRoboclawDrivetrain(const std::string& name, std::shared_ptr left, + std::shared_ptr right, std::shared_ptr navx = nullptr); ~TwoRoboclawDrivetrain(); @@ -45,7 +46,8 @@ namespace rip * * all range from [-1.0, 1.0] */ - virtual void drive(double front_left, double front_right, double back_left, double back_rightk) override; + virtual void drive(double front_left, double front_right, double back_left, + double back_rightk) override; /** * Single command to all motors @@ -60,7 +62,8 @@ namespace rip /** * Command four wheels separately */ - virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) override; + virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, + const MotorDynamics& back_left, const MotorDynamics& back_right) override; virtual void stop() override; diff --git a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp index 97da501..2158ee8 100644 --- a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp +++ b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp @@ -9,7 +9,10 @@ namespace rip { namespace drivetrains { - TwoRoboclawDrivetrain::TwoRoboclawDrivetrain(const std::string& name, std::shared_ptr left, std::shared_ptr right, std::shared_ptr navx) + TwoRoboclawDrivetrain::TwoRoboclawDrivetrain(const std::string& name, + std::shared_ptr left, + std::shared_ptr right, + std::shared_ptr navx) : Drivetrain(name) , m_left(left) , m_right(right) From 5c60a10e717fdc4c21efd27a5cd40a9db0a588ff Mon Sep 17 00:00:00 2001 From: solsane Date: Thu, 22 Feb 2018 12:31:43 -0500 Subject: [PATCH 004/200] turn to angle starts, navx and relative version --- .../navigation_actions/turn_to_angle.hpp | 51 ++++++++++-------- core/navigation/actions/src/turn_to_angle.cpp | 53 ++++++++++++++++--- 2 files changed, 76 insertions(+), 28 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index f8b6a7b..5fc8586 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -2,8 +2,11 @@ #define TURN_TO_ANGLE_HPP #include - +#include +#include #include +#include +#include namespace rip { @@ -11,43 +14,49 @@ namespace rip { namespace actions { + using NavX = navx::NavX; class TurnToAngle : public framework::Action { public: - + /** + * turns to angle, relative to current angle, by driving + * wheels in opposite directions at the same speed. + * @param name [description] + * @param drivetrain [description] + * @param speed [description] + * @param angle [description] + * @param navx [description] + * @param radius [description] + */ + TurnToAngle(const std::string& name, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, const units::Angle& angle, + std::shared_ptr navx, units::Distance radius); /** * Returns whether or not the action has finished execution. + * Returns true when navX reports change in angle that was + * requested */ - virtual bool isFinished() override - { - // todo - } - + virtual bool isFinished() override; /** * Iteratively called until {@see Action#isFinished()} returns true */ - virtual void update(nlohmann::json& state) override - { - // todo - } - + virtual void update(nlohmann::json& state) override; /** * Run once before the main code */ - virtual void setup(nlohmann::json& state) override - { - // todo - } - + virtual void setup(nlohmann::json& state) override; /** * Run once after finished */ - virtual void teardown(nlohmann::json& state) override - { - // todo - } + virtual void teardown(nlohmann::json& state) override; private: + units::AngularVelocity m_speed; + std::shared_ptr m_drivetrain; + std::shared_ptr m_navx; + units::Angle m_desiredAngle; + units::Distance m_c2wRadius; }; } } diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 4e9eb39..9d90530 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -1,4 +1,6 @@ #include "navigation_actions/turn_to_angle.hpp" +#include +#include namespace rip { @@ -6,24 +8,61 @@ namespace rip { namespace actions { - TurnToAngle::isFinished() + TurnToAngle::TurnToAngle(const std::string& name, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, const units::Angle& angle, + std::shared_ptr navx, units::Distance radius) + : Action(name) + , m_drivetrain(drivetrain) + , m_speed(speed) + , m_desiredAngle(angle) + , m_navx(navx) + , m_c2wRadius(radius) + {} + + bool TurnToAngle::isFinished() { + units::Angle currentAngle = m_navx->getAngle(); + if(m_desiredAngle >= 0) + { + return currentAngle >= m_desiredAngle; + } + else + { + return currentAngle <= m_desiredAngle; + } } - TurnToAngle::update() + void TurnToAngle::update(nlohmann::json& state) { - + return; } - TurnToAngle::setup() + void TurnToAngle::setup(nlohmann::json& state) { - + //initial angle + misc::Logger::getInstance()->debug("setting yaw offset"); + m_navx->zeroYaw(); + motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; + misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" + , m_speed.to(units::rev / units::minute))); + if(m_speed() < 0) + { + m_speed *= -1; + } + if(m_desiredAngle < 0) + { + m_speed *= -1; + } + dynamicsLeft.setSpeed(m_speed * m_c2wRadius / units::rad); + dynamicsRight.setSpeed(-1 * m_speed * m_c2wRadius / units::rad); + m_drivetrain->drive(dynamicsLeft, dynamicsRight); } - TurnToAngle::teardown() + void TurnToAngle::teardown(nlohmann::json& state) { - + m_drivetrain->stop(); } } } From 122a0728c707a1e5c3122461e58f7c5fa6a3fb91 Mon Sep 17 00:00:00 2001 From: solsane Date: Thu, 22 Feb 2018 15:04:09 -0500 Subject: [PATCH 005/200] minor doc & loggin --- .../include/navigation_actions/turn_to_angle.hpp | 13 +++++++------ core/navigation/actions/src/turn_to_angle.cpp | 3 +++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index 5fc8586..2bd2e6c 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -21,12 +21,13 @@ namespace rip /** * turns to angle, relative to current angle, by driving * wheels in opposite directions at the same speed. - * @param name [description] - * @param drivetrain [description] - * @param speed [description] - * @param angle [description] - * @param navx [description] - * @param radius [description] + * @param name name of action + * @param drivetrain drive train + * @param speed angular velocity of turn + * @param angle how many degrees t + * @param navx pointer to navx + * @param radius distance between wheel and the center of + * the robot (top down) */ TurnToAngle(const std::string& name, std::shared_ptr drivetrain, diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 9d90530..e6dab7a 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -55,6 +55,9 @@ namespace rip { m_speed *= -1; } + misc::Logger::getInstance()->debug(fmt::format("wheel linear speed (in/s): {}" + , (m_speed * m_c2wRadius / units::rad).to(units::in / units::s))); + dynamicsLeft.setSpeed(m_speed * m_c2wRadius / units::rad); dynamicsRight.setSpeed(-1 * m_speed * m_c2wRadius / units::rad); m_drivetrain->drive(dynamicsLeft, dynamicsRight); From 5b47db8129b97b5d1181c99e297a143f7b587bcf Mon Sep 17 00:00:00 2001 From: solsane Date: Thu, 22 Feb 2018 16:32:46 -0500 Subject: [PATCH 006/200] added static makeNavX function --- core/navigation/navx/include/navx/navx.hpp | 11 +++++++++++ core/navigation/navx/src/navx.cpp | 7 +++++++ 2 files changed, 18 insertions(+) diff --git a/core/navigation/navx/include/navx/navx.hpp b/core/navigation/navx/include/navx/navx.hpp index 8159791..e48b9de 100644 --- a/core/navigation/navx/include/navx/navx.hpp +++ b/core/navigation/navx/include/navx/navx.hpp @@ -6,6 +6,7 @@ #include #include #include +#include namespace rip { @@ -37,6 +38,7 @@ namespace rip * @author Scott & UTK IEEE Robotics. */ using namespace rip; + class NavX { public: @@ -755,6 +757,15 @@ namespace rip uint8_t getActualUpdateRateInternal(uint8_t update_rate); }; + + /** + * Creates & returns a pointer to a navX object, constructed based on parameters + * inside json. Used in RIP subsystem creation + * @param config json parameters for navX construction + * @return shared ptr to navX object + */ + static std::shared_ptr makeNavX(const nlohmann::json& config); + } // navx } // navigation } // rip diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index 7fbccb2..d4ae21d 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include #include @@ -30,6 +31,12 @@ namespace rip static const uint8_t NAVX_MXP_I2C_ADDRESS = 0x32; static const float QUATERNION_HISTORY_SECONDS = 5.0f; + std::shared_ptr makeNavX(const nlohmann::json& config) + { + std::string device = config["device"]; + return std::make_shared(device); + } + class NavXInternal : public IIOCompleteNotification, public IBoardCapabilities { NavX* navx; From 21a0dd04086a17f9631791c83f15b5ed1d650979 Mon Sep 17 00:00:00 2001 From: Parker Mitchell Date: Fri, 23 Feb 2018 21:20:47 -0500 Subject: [PATCH 007/200] Start of a simple PID control utility --- core/navigation/actions/CMakeLists.txt | 2 +- core/utilities/CMakeLists.txt | 1 + core/utilities/pid/CMakeLists.txt | 12 ++++ core/utilities/pid/include/pid/pid.hpp | 82 ++++++++++++++++++++++++++ core/utilities/pid/src/pid.cpp | 55 +++++++++++++++++ 5 files changed, 151 insertions(+), 1 deletion(-) create mode 100644 core/utilities/pid/CMakeLists.txt create mode 100644 core/utilities/pid/include/pid/pid.hpp create mode 100644 core/utilities/pid/src/pid.cpp diff --git a/core/navigation/actions/CMakeLists.txt b/core/navigation/actions/CMakeLists.txt index 085d3cb..64e3098 100644 --- a/core/navigation/actions/CMakeLists.txt +++ b/core/navigation/actions/CMakeLists.txt @@ -7,5 +7,5 @@ file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -target_link_libraries(${PROJECT_NAME} framework drivetrains motor_controllers navx) +target_link_libraries(${PROJECT_NAME} framework drivetrains motor_controllers navx pid) target_include_directories(${PROJECT_NAME} PUBLIC include) diff --git a/core/utilities/CMakeLists.txt b/core/utilities/CMakeLists.txt index f32fe6c..f05c872 100644 --- a/core/utilities/CMakeLists.txt +++ b/core/utilities/CMakeLists.txt @@ -6,3 +6,4 @@ add_subdirectory(cmd_messenger) add_subdirectory(units) add_subdirectory(misc) add_subdirectory(geometry) +add_subdirectory(pid) diff --git a/core/utilities/pid/CMakeLists.txt b/core/utilities/pid/CMakeLists.txt new file mode 100644 index 0000000..68babac --- /dev/null +++ b/core/utilities/pid/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.1) +message("Building PidController") +# define the project +project(pid) + +# create and configure the library target +file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cpp) +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/pid/*.hpp) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) +target_link_libraries(${PROJECT_NAME} units) +target_include_directories(${PROJECT_NAME} PUBLIC include) diff --git a/core/utilities/pid/include/pid/pid.hpp b/core/utilities/pid/include/pid/pid.hpp new file mode 100644 index 0000000..3b54a3c --- /dev/null +++ b/core/utilities/pid/include/pid/pid.hpp @@ -0,0 +1,82 @@ +#ifndef PID_HPP +#define PID_HPP + +#include +#include + +namespace rip +{ + namespace utilities + { + /** + * A Propotional, Integral, Derivative Controller + */ + class PidController + { + public: + /** + * Constructor + * + * @param p proporitional constant (K_p) + * @param i integral constant (K_i) + * @param d derivative constant (K_d) + */ + PidController(const double p = 0.0, const double i = 0.0, const double d = 0.0) : + m_p(p), m_i(i), m_d(d), m_target(0), m_last_error(0), m_accum_error(0), + m_last_time(std::chrono::high_resolution_clock::now()) {} + + /** + * setParams + * + * @param p proporitional constant (K_p) + * @param i integral constant (K_i) + * @param d derivative constant (K_d) + */ + void setParams(const double p, const double i, const double d); + + /** + * Constructor + * + * @return A tuple of the three PID parameters in order (p, i, d) + */ + std::tuple getParams(); + + + /** + * setTarget + * + * @param target The target value for PID process + */ + void setTarget(double target); + + /** + * getTarget + * + * @return The target setpoint as a double + */ + double getTarget(); + + /** + * update + * + * @brief The main update loop for the PID + * @param val The input value for the PID used for error calculation + * @return The updated output control signal + */ + double update(double val); + + private: + double m_p; + double m_i; + double m_d; + double m_target; + double m_last_error; + double m_accum_error; + std::chrono::high_resolution_clock::time_point m_last_time; + + }; // class PidController + + } // namespace utilities +} // namespace rip + +#endif diff --git a/core/utilities/pid/src/pid.cpp b/core/utilities/pid/src/pid.cpp new file mode 100644 index 0000000..3f41c1c --- /dev/null +++ b/core/utilities/pid/src/pid.cpp @@ -0,0 +1,55 @@ +#include "pid/pid.hpp" + +namespace rip +{ + namespace utilities + { + + void PidController::setParams(double p, double i, double d) + { + m_p = p; + m_i = i; + m_d = d; + } + + std::tuple PidController::getParams() + { + return std::make_tuple(m_p, m_i, m_d); + } + + double PidController::getTarget() + { + return m_target; + } + + void PidController::setTarget(double target) + { + m_target = target; + } + + double PidController::update(double val) + { + // Calculate the change in time since the last update loop + std::chrono::high_resolution_clock::time_point time_now = std::chrono::high_resolution_clock::now(); + std::chrono::duration dt = time_now - m_last_time; + m_last_time = time_now; + + // Calculate the error + double error = m_target - val; + + // Proportional Term (K_p * e(t)) + double p_term = m_p * error; + + // Integral Term (K_i * [e(t) dt from 0 to t]) + m_accum_error += error; + double i_term = m_i * m_accum_error; + + // Derivative Term (K_d * de(t)/dt) + double d_term = m_d * ((error - m_last_error) / dt.count()); + m_last_error = error; + + // Return the sum of the PID components + return p_term + i_term + d_term; + } + } +} From aaf989cab13fda61111909966d0437580650ab9d Mon Sep 17 00:00:00 2001 From: Parker Mitchell Date: Sun, 25 Feb 2018 14:13:18 -0500 Subject: [PATCH 008/200] pid reset --- core/utilities/pid/include/pid/pid.hpp | 10 ++++++++-- core/utilities/pid/src/pid.cpp | 7 +++++++ 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/core/utilities/pid/include/pid/pid.hpp b/core/utilities/pid/include/pid/pid.hpp index 3b54a3c..42d0f19 100644 --- a/core/utilities/pid/include/pid/pid.hpp +++ b/core/utilities/pid/include/pid/pid.hpp @@ -35,13 +35,12 @@ namespace rip void setParams(const double p, const double i, const double d); /** - * Constructor + * getParams * * @return A tuple of the three PID parameters in order (p, i, d) */ std::tuple getParams(); - /** * setTarget * @@ -65,6 +64,13 @@ namespace rip */ double update(double val); + /** + * reset + * + * @brief Resets the internal error calculation + */ + void reset(); + private: double m_p; double m_i; diff --git a/core/utilities/pid/src/pid.cpp b/core/utilities/pid/src/pid.cpp index 3f41c1c..6422fb7 100644 --- a/core/utilities/pid/src/pid.cpp +++ b/core/utilities/pid/src/pid.cpp @@ -51,5 +51,12 @@ namespace rip // Return the sum of the PID components return p_term + i_term + d_term; } + + void PidController::reset() + { + m_accum_error = 0; + m_last_error = 0; + m_last_time = std::chrono::high_resolution_clock::now(); + } } } From 923c98e846ba9a50421bccc638430d45471cb2b4 Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 25 Feb 2018 17:35:57 -0500 Subject: [PATCH 009/200] add encoder abstraction to drivetrain.hpp --- .../include/navigation_actions/drive_arc.hpp | 24 +++- .../navigation_actions/turn_to_angle.hpp | 3 +- core/navigation/actions/src/drive_arc.cpp | 35 ++++- core/navigation/actions/src/turn_to_angle.cpp | 4 +- .../include/drivetrains/drivetrain.hpp | 34 ++++- .../include/drivetrains/exceptions.hpp | 11 ++ .../drivetrains/two_roboclaw_drivetrain.hpp | 25 +++- .../src/two_roboclaw_drivetrain.cpp | 129 +++++++++++++++++- 8 files changed, 253 insertions(+), 12 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index 818847c..8dc6507 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -15,7 +15,20 @@ namespace rip { public: - + /** + * [DriveArc description] + * @param name Action name + * @param direction 0 means left turn. 1 means right turn + * @param drivetrain pointer to differential drivetrain. + * @param speed angular velocity for the turn + * @param angle between + * @param radius size of arc + * @param axleLength width between left and right wheels. + */ + DriveArc(const std::string& name, bool direction, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, const units::Angle& angle, + units::Distance& radius, units::Distance& axleLength); /** * Returns whether or not the action has finished execution. @@ -38,6 +51,15 @@ namespace rip virtual void teardown(nlohmann::json& state) override; private: + units::Angle m_desiredAngle; + std::shared_ptr m_drivetrain; + /** + * radius is distance from center of robot/differential drivetrain + * to center of circle of arc (ICC) + */ + units::Distance m_axleLength, m_radius, m_arcLength; + units::AngularVelocity m_speed; + bool m_direction; }; } } diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index 2bd2e6c..bcf664a 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -14,6 +14,7 @@ namespace rip { namespace actions { + //currently depends on differential drivetrain using NavX = navx::NavX; class TurnToAngle : public framework::Action { @@ -32,7 +33,7 @@ namespace rip TurnToAngle(const std::string& name, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - std::shared_ptr navx, units::Distance radius); + std::shared_ptr navx, units::Distance& radius); /** * Returns whether or not the action has finished execution. * Returns true when navX reports change in angle that was diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 84a269d..a2e5c77 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -6,19 +6,50 @@ namespace rip { namespace actions { + DriveArc::DriveArc(const std::string& name, bool direction, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, const units::Angle& angle, + units::Distance& radius, units::Distance& axleLength) + : Action(name) + , m_direction(direction) + , m_drivetrain(drivetrain) + , m_speed(speed) + , m_angle(angle) + , m_radius(radius) + , m_axleLength(axleLength) + { + m_arcLength = angle * radius; + } + + DriveArc::DriveArc(const std::string& name, bool direction, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, const units::Distance& arcLength, + units::Distance& radius, units::Distance& axleLength) + : Action(name) + , m_direction(direction) + , m_drivetrain(drivetrain) + , m_speed(speed) + , m_arcLength(arcLength) + , m_radius(radius) + , m_axleLength(axleLength) + { + m_angle = arcLength / radius; + } + bool DriveArc::isFinished() { - // todo + return m_arcLength >= } void DriveArc::update(nlohmann::json& state) { // todo + return; } void DriveArc::setup(nlohmann::json& state) { - // todo + m_traveled = m_drivetrain } void DriveArc::teardown(nlohmann::json& state) diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index e6dab7a..cf87520 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -11,7 +11,7 @@ namespace rip TurnToAngle::TurnToAngle(const std::string& name, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - std::shared_ptr navx, units::Distance radius) + std::shared_ptr navx, units::Distance& radius) : Action(name) , m_drivetrain(drivetrain) , m_speed(speed) @@ -57,7 +57,7 @@ namespace rip } misc::Logger::getInstance()->debug(fmt::format("wheel linear speed (in/s): {}" , (m_speed * m_c2wRadius / units::rad).to(units::in / units::s))); - + dynamicsLeft.setSpeed(m_speed * m_c2wRadius / units::rad); dynamicsRight.setSpeed(-1 * m_speed * m_c2wRadius / units::rad); m_drivetrain->drive(dynamicsLeft, dynamicsRight); diff --git a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp index fd81645..feac97e 100644 --- a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp +++ b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp @@ -15,6 +15,18 @@ namespace rip */ class Drivetrain : public framework::Subsystem { + enum class Motor + { + //4 motor drivetrain + kFrontLeft, + kFrontRight, + kBackLeft, + kBackRight, + //2 motor drive train + kLeft, + kRight + }; // enum class Motor + using MotorDynamics = motorcontrollers::MotorDynamics; public: Drivetrain(const std::string& name) @@ -55,8 +67,28 @@ namespace rip /** * Command four wheels separately */ - virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, + virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) = 0; + + /** + * Reads encoders for every motor you tell it to read, reports back in respective + * order + */ + virtual std::vector readEncoders(const std::vector& motors) = 0; + /** + * reads the encoder for one motor + */ + virtual units::Distance readEncoder(const Motor& motor) = 0; + /** + * Reads encoder velocity for every motor you tell it to read, reports back in respective + * order + * @param motors list of motors to read + */ + virtual std::vector readEncoderVelocities(const std::vector& motors) = 0; + /** + * reads the encoder for one motor + */ + virtual units::Velocity readEncoderVelocity(const Motor& motor) = 0; }; } } diff --git a/core/navigation/drivetrains/include/drivetrains/exceptions.hpp b/core/navigation/drivetrains/include/drivetrains/exceptions.hpp index 2d376d6..163fbc9 100644 --- a/core/navigation/drivetrains/include/drivetrains/exceptions.hpp +++ b/core/navigation/drivetrains/include/drivetrains/exceptions.hpp @@ -9,7 +9,18 @@ namespace rip { namespace drivetrains { + /** + * Provided value is out of range + * @param OutOfRangeException [description] + */ NEW_EX(OutOfRangeException) + /** + * attempt to use a motor that does not exist, + * ie requesting encoder informoation from back right motor in a 2 wheel configuration + * @param InvalidEncoderRequest [description] + */ + NEW_EX(InvalidMotorException) + } } } diff --git a/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp index e2cc4db..ab7b895 100644 --- a/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp +++ b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_drivetrain.hpp @@ -5,7 +5,7 @@ #include #include - +#include #include "drivetrain.hpp" namespace rip @@ -17,6 +17,7 @@ namespace rip /** * Abstract base class for the drive train */ + class TwoRoboclawDrivetrain : public Drivetrain { using Roboclaw = motorcontrollers::roboclaw::Roboclaw; @@ -63,13 +64,33 @@ namespace rip * Command four wheels separately */ virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, - const MotorDynamics& back_left, const MotorDynamics& back_right) override; + const MotorDynamics& back_left, const MotorDynamics& back_right) override; + /** + * Reads encoders for every motor you tell it to read, reports back in respective + * order + */ + virtual std::vector readEncoders(const std::vector& motors) override; + /** + * reads the encoder for one motor + */ + virtual units::Distance readEncoder(const Motor& motor) override; + /** + * Reads encoder velocity for every motor you tell it to read, reports back in respective + * order + * @param motors list of motors to read + */ + virtual std::vector readEncoderVelocities(const std::vector& motors) override; + /** + * reads the encoder for one motor + */ + virtual units::Velocity readEncoderVelocity(const Motor& motor) override; virtual void stop() override; virtual bool diagnostic() override; private: + std::shared_ptr m_left; std::shared_ptr m_right; std::shared_ptr m_navx; diff --git a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp index 2158ee8..c7e2fe8 100644 --- a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp +++ b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp @@ -44,9 +44,11 @@ namespace rip m_right->drive(32767 * right); } - void TwoRoboclawDrivetrain::drive(double front_left, double front_right, double back_left, double back_right) + void TwoRoboclawDrivetrain::drive(double front_left, double front_right, double back_left, + double back_right) { - if (std::abs(front_left) > 1 || std::abs(front_right) > 1 || std::abs(back_left) > 1 || std::abs(back_right) > 1) + if (std::abs(front_left) > 1 || std::abs(front_right) > 1 || std::abs(back_left) > 1 || + std::abs(back_right) > 1) { throw OutOfRangeException("out of range"); } @@ -70,7 +72,8 @@ namespace rip m_right->setDynamics(right); } - void TwoRoboclawDrivetrain::drive(const MotorDynamics& front_left, const MotorDynamics& front_right, const MotorDynamics& back_left, const MotorDynamics& back_right) + void TwoRoboclawDrivetrain::drive(const MotorDynamics& front_left, const MotorDynamics& front_right, + const MotorDynamics& back_left, const MotorDynamics& back_right) { m_left->setDynamics(Roboclaw::Motor::kM1, front_left); m_right->setDynamics(Roboclaw::Motor::kM1, front_right); @@ -80,6 +83,125 @@ namespace rip } + std::vector TwoRoboclawDrivetrain::readEncoders(const std::vector& motors) + { + std::vector data; + for(int i=0; ireadEncoder(Roboclaw::Motor::kM1)); + } + case kFrontRight: + { + data.push_back(m_right->readEncoder(Roboclaw::Motor::kM1)); + } + case kBackLeft: + { + data.push_back(m_left->readEncoder(Roboclaw::Motor::kM2)); + } + case kBackRight: + { + data.push_back(m_right->readEncoder(Roboclaw::Motor::kM2)); + } + default: + { + throw InvalidMotorException("Invalid motor"); + } + } + } + return data; + } + + units::Distance TwoRoboclawDrivetrain::readEncoder(const Motor& motor) + { + switch(motor) + { + case kFrontLeft: + { + return m_left->readEncoder(Roboclaw::Motor::kM1); + } + case kFrontRight: + { + return m_right->readEncoder(Roboclaw::Motor::kM1)); + } + case kBackLeft: + { + return m_left->readEncoder(Roboclaw::Motor::kM2)); + } + case kBackRight: + { + return m_right->readEncoder(Roboclaw::Motor::kM2)); + } + default: + { + throw InvalidMotorException("Invalid motor"); + } + } + } + + std::vector TwoRoboclawDrivetrain::readEncoderVelocities( + const std::vector& motors) + { + std::vector data; + for(int i=0; ireadEncoderVelocity(Roboclaw::Motor::kM1)); + } + case kFrontRight: + { + data.push_back(m_right->readEncoderVelocity(Roboclaw::Motor::kM1)); + } + case kBackLeft: + { + data.push_back(m_left->readEncoderVelocity(Roboclaw::Motor::kM2)); + } + case kBackRight: + { + data.push_back(m_right->readEncoderVelocity(Roboclaw::Motor::kM2)); + } + default: + { + throw InvalidMotorException("Invalid motor") + } + } + } + return data; + } + + units::Velocity TwoRoboclawDrivetrain::readEncoderVelocity(const Motor& motor) + { + switch(motor) + { + case kFrontLeft: + { + return m_left->readEncoderVelocity(Roboclaw::Motor::kM1); + } + case kFrontRight: + { + return m_right->readEncoderVelocity(Roboclaw::Motor::kM1)); + } + case kBackLeft: + { + return m_left->readEncoderVelocity(Roboclaw::Motor::kM2)); + } + case kBackRight: + { + return m_right->readEncoderVelocity(Roboclaw::Motor::kM2)); + } + default: + { + throw InvalidMotorException("Invalid motor"); + } + } + } + void TwoRoboclawDrivetrain::stop() { m_left->drive(0); @@ -89,6 +211,7 @@ namespace rip bool TwoRoboclawDrivetrain::diagnostic() { // todo + return 0; } } }//subsystem From 7c15d4b843c2b4a8e85820a2b133e3d66e1714a0 Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 25 Feb 2018 17:38:14 -0500 Subject: [PATCH 010/200] added one roboclaw drive train --- .../drivetrains/one_roboclaw_drivetrain.hpp | 84 +++++++++++++++++++ .../src/one_roboclaw_drivetrain.cpp | 68 +++++++++++++++ 2 files changed, 152 insertions(+) create mode 100644 core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp create mode 100644 core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp diff --git a/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp new file mode 100644 index 0000000..4430608 --- /dev/null +++ b/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp @@ -0,0 +1,84 @@ +#ifndef ONE_ROBOCLAW_DRIVE_TRAIN_HPP +#define ONE_ROBOCLAW_DRIVE_TRAIN_HPP + +#include + +#include +#include +#include +#include "drivetrain.hpp" + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + /** + * Abstract base class for the drive train + */ + class TwoRoboclawDrivetrain : public Drivetrain + { + using Roboclaw = motorcontrollers::roboclaw::Roboclaw; + using NavX = navx::NavX; + using MotorDynamics = motorcontrollers::MotorDynamics; + public: + OneRoboclawDrivetrain(const std::string& name, std::shared_ptr rclaw, + std::shared_ptr navx = nullptr); + + ~OneRoboclawDrivetrain(); + + /** + * Drive all the motors + * @param power [-1.0, 1.0] + */ + virtual void drive(double power) override; + + /** + * Drive left and right separately + * @param left [-1.0, 1.0] + * @param right [-1.0, 1.0] + */ + virtual void drive(double left, double right) override; + + /** + * Single command to all motors + */ + virtual void drive(const MotorDynamics& command) override; + + /** + * Command left and right sides separately + */ + virtual void drive(const MotorDynamics& left, const MotorDynamics& right) override; + /** + * Reads encoders for every motor you tell it to read, reports back in respective + * order + */ + virtual std::vector readEncoders(const std::vector& motors) override; + /** + * reads the encoder for one motor + */ + virtual units::Distance readEncoder(const Motor& motor) override; + /** + * Reads encoder velocity for every motor you tell it to read, reports back in respective + * order + * @param motors list of motors to read + */ + virtual std::vector readEncoderVelocities(const std::vector& motors) override; + /** + * reads the encoder for one motor + */ + virtual units::Velocity readEncoderVelocity(const Motor& motor) override; + + virtual void stop() override; + + virtual bool diagnostic() override; + private: + std::shared_ptr m_rclaw; + std::shared_ptr m_navx; + }; + } + } + } + + #endif // DRIVE_TRAIN_HPP diff --git a/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp new file mode 100644 index 0000000..95c2059 --- /dev/null +++ b/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp @@ -0,0 +1,68 @@ +#include "drivetrains/one_roboclaw_drivetrain.hpp" +#include "drivetrains/two_roboclaw_drivetrain.hpp" +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + TwoRoboclawDrivetrain::TwoRoboclawDrivetrain(const std::string& name, + std::shared_ptr rclaw, + std::shared_ptr navx) + : Drivetrain(name) + , m_rclaw(rclaw) + , m_navx(navx) + {} + + TwoRoboclawDrivetrain::~TwoRoboclawDrivetrain() + { + stop(); + } + + void TwoRoboclawDrivetrain::drive(double power) + { + if (std::abs(power) > 1) + { + throw OutOfRangeException("out of range"); + } + m_rclaw->drive(32767 * power); + } + + void TwoRoboclawDrivetrain::drive(double left, double right) + { + if (std::abs(left) > 1 || std::abs(right) > 1) + { + throw OutOfRangeException("out of range"); + } + m_rclaw->drive(Roboclaw::Motor::kM1, 32767 * left); + m_rclaw->drive(Roboclaw::Motor::kM2, 32767 * right); + } + + void TwoRoboclawDrivetrain::drive(const MotorDynamics& command) + { + m_rclaw->setDynamics(command); + } + + void TwoRoboclawDrivetrain::drive(const MotorDynamics& left, const MotorDynamics& right) + { + m_rclaw->setDynamics(Robclaw::Motor::kM1, left); + m_rclaw->setDynamics(Robclaw::Motor::kM2, right); + } + + void TwoRoboclawDrivetrain::stop() + { + m_rclaw->drive(0); + } + + bool TwoRoboclawDrivetrain::diagnostic() + { + // todo + return 0; + } + } + } +} From 594d73472e4abefe57c0e87dcb78bd753491871d Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 25 Feb 2018 21:17:14 -0500 Subject: [PATCH 011/200] added drive arc, among other things. compiles. --- .../include/navigation_actions/drive_arc.hpp | 38 +++++++++++---- core/navigation/actions/src/drive_arc.cpp | 24 ++++++++-- .../include/drivetrains/drivetrain.hpp | 15 +++--- .../drivetrains/one_roboclaw_drivetrain.hpp | 2 +- .../src/one_roboclaw_drivetrain.cpp | 25 +++++----- .../src/two_roboclaw_drivetrain.cpp | 46 +++++++++---------- 6 files changed, 94 insertions(+), 56 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index 8dc6507..e28e0f6 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -2,8 +2,10 @@ #define DRIVE_ARC_HPP #include - +#include +#include #include +#include namespace rip { @@ -11,18 +13,20 @@ namespace rip { namespace actions { + using Motor = rip::navigation::drivetrains::Drivetrain::Motor; class DriveArc : public framework::Action { public: /** - * [DriveArc description] + * Drives the robot in an arc with specified radius, angular distance, + * angular velocity for the turn * @param name Action name * @param direction 0 means left turn. 1 means right turn * @param drivetrain pointer to differential drivetrain. * @param speed angular velocity for the turn * @param angle between - * @param radius size of arc + * @param radius radius size of arc turn * @param axleLength width between left and right wheels. */ DriveArc(const std::string& name, bool direction, @@ -30,34 +34,52 @@ namespace rip const units::AngularVelocity& speed, const units::Angle& angle, units::Distance& radius, units::Distance& axleLength); + /** + * Drives the robot in an arc with specified radius, arc length, + * angular velocity for the turn + * @param name Action name + * @param direction 0 means left turn. 1 means right turn + * @param drivetrain pointer to differential drivetrain. + * @param speed angular velocity for the turn + * @param arcLength Linear distance that for the robot to travel + * @param radius radius size of arc turn + * @param axleLength width between left and right wheels. + */ + DriveArc(const std::string& name, bool direction, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, const units::Distance& arcLength, + units::Distance& radius, units::Distance& axleLength); /** * Returns whether or not the action has finished execution. */ virtual bool isFinished() override; - /** * Iteratively called until {@see Action#isFinished()} returns true */ virtual void update(nlohmann::json& state) override; - /** * Run once before the main code */ virtual void setup(nlohmann::json& state) override; - + /** + * //TODO: support for multiple drivetrain types + * Returns the average distance read from all four wheels + * @return average distance + */ + units::Distance readAverageDistance(); /** * Run once after finished */ virtual void teardown(nlohmann::json& state) override; private: - units::Angle m_desiredAngle; + units::Angle m_angle; std::shared_ptr m_drivetrain; /** * radius is distance from center of robot/differential drivetrain * to center of circle of arc (ICC) */ - units::Distance m_axleLength, m_radius, m_arcLength; + units::Distance m_axleLength, m_radius, m_arcLength, m_traveled, m_init; units::AngularVelocity m_speed; bool m_direction; }; diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index a2e5c77..5ef8139 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -18,7 +18,7 @@ namespace rip , m_radius(radius) , m_axleLength(axleLength) { - m_arcLength = angle * radius; + m_arcLength = angle.to(units::rad) * radius; } DriveArc::DriveArc(const std::string& name, bool direction, @@ -33,12 +33,13 @@ namespace rip , m_radius(radius) , m_axleLength(axleLength) { - m_angle = arcLength / radius; + m_angle = arcLength / radius * units::rad; } bool DriveArc::isFinished() { - return m_arcLength >= + m_traveled = readAverageDistance(); + return m_arcLength >= (m_traveled - m_init); } void DriveArc::update(nlohmann::json& state) @@ -49,12 +50,27 @@ namespace rip void DriveArc::setup(nlohmann::json& state) { - m_traveled = m_drivetrain + std::vector dist; + m_init = readAverageDistance(); + } + + units::Distance DriveArc::readAverageDistance() + { + units::Distance sum=0; + std::vector dist = m_drivetrain->readEncoders({Motor::kFrontLeft, + Motor::kFrontLeft, Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}); + for(int i=0; i(dist.size()); i++) + { + sum += dist[i]; + } + sum /= dist.size(); + return sum; } void DriveArc::teardown(nlohmann::json& state) { // todo + m_drivetrain->stop(); } } } diff --git a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp index feac97e..2b481f3 100644 --- a/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp +++ b/core/navigation/drivetrains/include/drivetrains/drivetrain.hpp @@ -15,9 +15,16 @@ namespace rip */ class Drivetrain : public framework::Subsystem { + + using MotorDynamics = motorcontrollers::MotorDynamics; + public: + Drivetrain(const std::string& name) + : Subsystem(name) + {} + enum class Motor { - //4 motor drivetrain + //4 motor drivetrain kFrontLeft, kFrontRight, kBackLeft, @@ -27,12 +34,6 @@ namespace rip kRight }; // enum class Motor - using MotorDynamics = motorcontrollers::MotorDynamics; - public: - Drivetrain(const std::string& name) - : Subsystem(name) - {} - /** * Drive all the motors * @param power [-1, 1] diff --git a/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp index 4430608..61aff2b 100644 --- a/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp +++ b/core/navigation/drivetrains/include/drivetrains/one_roboclaw_drivetrain.hpp @@ -17,7 +17,7 @@ namespace rip /** * Abstract base class for the drive train */ - class TwoRoboclawDrivetrain : public Drivetrain + class OneRoboclawDrivetrain : public Drivetrain { using Roboclaw = motorcontrollers::roboclaw::Roboclaw; using NavX = navx::NavX; diff --git a/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp index 95c2059..bbfa0b4 100644 --- a/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp +++ b/core/navigation/drivetrains/src/one_roboclaw_drivetrain.cpp @@ -1,5 +1,4 @@ #include "drivetrains/one_roboclaw_drivetrain.hpp" -#include "drivetrains/two_roboclaw_drivetrain.hpp" #include #include #include @@ -10,20 +9,20 @@ namespace rip { namespace drivetrains { - TwoRoboclawDrivetrain::TwoRoboclawDrivetrain(const std::string& name, - std::shared_ptr rclaw, - std::shared_ptr navx) + OneRoboclawDrivetrain::OneRoboclawDrivetrain(const std::string& name, + std::shared_ptr rclaw, + std::shared_ptr navx) : Drivetrain(name) , m_rclaw(rclaw) , m_navx(navx) {} - TwoRoboclawDrivetrain::~TwoRoboclawDrivetrain() + OneRoboclawDrivetrain::~OneRoboclawDrivetrain() { stop(); } - void TwoRoboclawDrivetrain::drive(double power) + void OneRoboclawDrivetrain::drive(double power) { if (std::abs(power) > 1) { @@ -32,7 +31,7 @@ namespace rip m_rclaw->drive(32767 * power); } - void TwoRoboclawDrivetrain::drive(double left, double right) + void OneRoboclawDrivetrain::drive(double left, double right) { if (std::abs(left) > 1 || std::abs(right) > 1) { @@ -42,23 +41,23 @@ namespace rip m_rclaw->drive(Roboclaw::Motor::kM2, 32767 * right); } - void TwoRoboclawDrivetrain::drive(const MotorDynamics& command) + void OneRoboclawDrivetrain::drive(const MotorDynamics& command) { m_rclaw->setDynamics(command); } - void TwoRoboclawDrivetrain::drive(const MotorDynamics& left, const MotorDynamics& right) + void OneRoboclawDrivetrain::drive(const MotorDynamics& left, const MotorDynamics& right) { - m_rclaw->setDynamics(Robclaw::Motor::kM1, left); - m_rclaw->setDynamics(Robclaw::Motor::kM2, right); + m_rclaw->setDynamics(Roboclaw::Motor::kM1, left); + m_rclaw->setDynamics(Roboclaw::Motor::kM2, right); } - void TwoRoboclawDrivetrain::stop() + void OneRoboclawDrivetrain::stop() { m_rclaw->drive(0); } - bool TwoRoboclawDrivetrain::diagnostic() + bool OneRoboclawDrivetrain::diagnostic() { // todo return 0; diff --git a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp index c7e2fe8..d426576 100644 --- a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp +++ b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp @@ -90,19 +90,19 @@ namespace rip { switch(motors[i]) { - case kFrontLeft: + case Motor::kFrontLeft: { data.push_back(m_left->readEncoder(Roboclaw::Motor::kM1)); } - case kFrontRight: + case Motor::kFrontRight: { data.push_back(m_right->readEncoder(Roboclaw::Motor::kM1)); } - case kBackLeft: + case Motor::kBackLeft: { data.push_back(m_left->readEncoder(Roboclaw::Motor::kM2)); } - case kBackRight: + case Motor::kBackRight: { data.push_back(m_right->readEncoder(Roboclaw::Motor::kM2)); } @@ -119,21 +119,21 @@ namespace rip { switch(motor) { - case kFrontLeft: + case Motor::kFrontLeft: { return m_left->readEncoder(Roboclaw::Motor::kM1); } - case kFrontRight: + case Motor::kFrontRight: { - return m_right->readEncoder(Roboclaw::Motor::kM1)); + return m_right->readEncoder(Roboclaw::Motor::kM1); } - case kBackLeft: + case Motor::kBackLeft: { - return m_left->readEncoder(Roboclaw::Motor::kM2)); + return m_left->readEncoder(Roboclaw::Motor::kM2); } - case kBackRight: + case Motor::kBackRight: { - return m_right->readEncoder(Roboclaw::Motor::kM2)); + return m_right->readEncoder(Roboclaw::Motor::kM2); } default: { @@ -150,25 +150,25 @@ namespace rip { switch(motors[i]) { - case kFrontLeft: + case Motor::kFrontLeft: { data.push_back(m_left->readEncoderVelocity(Roboclaw::Motor::kM1)); } - case kFrontRight: + case Motor::kFrontRight: { data.push_back(m_right->readEncoderVelocity(Roboclaw::Motor::kM1)); } - case kBackLeft: + case Motor::kBackLeft: { data.push_back(m_left->readEncoderVelocity(Roboclaw::Motor::kM2)); } - case kBackRight: + case Motor::kBackRight: { data.push_back(m_right->readEncoderVelocity(Roboclaw::Motor::kM2)); } default: { - throw InvalidMotorException("Invalid motor") + throw InvalidMotorException("Invalid motor"); } } } @@ -179,21 +179,21 @@ namespace rip { switch(motor) { - case kFrontLeft: + case Motor::kFrontLeft: { return m_left->readEncoderVelocity(Roboclaw::Motor::kM1); } - case kFrontRight: + case Motor::kFrontRight: { - return m_right->readEncoderVelocity(Roboclaw::Motor::kM1)); + return m_right->readEncoderVelocity(Roboclaw::Motor::kM1); } - case kBackLeft: + case Motor::kBackLeft: { - return m_left->readEncoderVelocity(Roboclaw::Motor::kM2)); + return m_left->readEncoderVelocity(Roboclaw::Motor::kM2); } - case kBackRight: + case Motor::kBackRight: { - return m_right->readEncoderVelocity(Roboclaw::Motor::kM2)); + return m_right->readEncoderVelocity(Roboclaw::Motor::kM2); } default: { From f48133659ed75e3f4c07b267110edec9dea3e92d Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 25 Feb 2018 22:16:46 -0500 Subject: [PATCH 012/200] drive arc can be tested now; resolving some warnings --- .../include/navigation_actions/drive_arc.hpp | 13 ++++---- core/navigation/actions/src/drive_arc.cpp | 32 +++++++++++++++++-- 2 files changed, 36 insertions(+), 9 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index e28e0f6..9fc4daa 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -25,7 +25,7 @@ namespace rip * @param direction 0 means left turn. 1 means right turn * @param drivetrain pointer to differential drivetrain. * @param speed angular velocity for the turn - * @param angle between + * @param angle angle to tunr * @param radius radius size of arc turn * @param axleLength width between left and right wheels. */ @@ -47,7 +47,7 @@ namespace rip */ DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, - const units::AngularVelocity& speed, const units::Distance& arcLength, + const units::Velocity& speed, const units::Distance& arcLength, units::Distance& radius, units::Distance& axleLength); /** * Returns whether or not the action has finished execution. @@ -73,15 +73,16 @@ namespace rip virtual void teardown(nlohmann::json& state) override; private: - units::Angle m_angle; + bool m_direction; std::shared_ptr m_drivetrain; + units::AngularVelocity m_angularSpeed; + units::Velocity m_linearSpeed; + units::Angle m_angle; /** * radius is distance from center of robot/differential drivetrain * to center of circle of arc (ICC) */ - units::Distance m_axleLength, m_radius, m_arcLength, m_traveled, m_init; - units::AngularVelocity m_speed; - bool m_direction; + units::Distance m_arcLength, m_radius, m_axleLength, m_traveled, m_init; }; } } diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 5ef8139..9c6227c 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -13,27 +13,29 @@ namespace rip : Action(name) , m_direction(direction) , m_drivetrain(drivetrain) - , m_speed(speed) + , m_angularSpeed(speed) , m_angle(angle) , m_radius(radius) , m_axleLength(axleLength) { m_arcLength = angle.to(units::rad) * radius; + m_linearSpeed = radius * speed / units::rad; } DriveArc::DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, - const units::AngularVelocity& speed, const units::Distance& arcLength, + const units::Velocity& speed, const units::Distance& arcLength, units::Distance& radius, units::Distance& axleLength) : Action(name) , m_direction(direction) , m_drivetrain(drivetrain) - , m_speed(speed) + , m_linearSpeed(speed) , m_arcLength(arcLength) , m_radius(radius) , m_axleLength(axleLength) { m_angle = arcLength / radius * units::rad; + m_angularSpeed = speed / radius * units::rad; } bool DriveArc::isFinished() @@ -51,7 +53,31 @@ namespace rip void DriveArc::setup(nlohmann::json& state) { std::vector dist; + units::Velocity v1, v2; m_init = readAverageDistance(); + + misc::Logger::getInstance()->debug(fmt::format("initial(offset) distance(ft): {}", m_init.to(units::ft))); + motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; + + misc::Logger::getInstance()->debug(fmt::format("arc turn intended angular velocity (rev/min): {}" + , m_angularSpeed.to(units::rev / units::minute))); + + misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear velocity(in/s): {}" + , m_linearSpeed.to(units::in / units::s))); + + v1 = m_angularSpeed * (m_radius + m_axleLength/2 ) / units::rad; + v2 = m_angularSpeed * (m_radius - m_axleLength/2 ) / units::rad; + if(!m_direction) // left turn + { + dynamicsLeft.setSpeed(v2); + dynamicsRight.setSpeed(v1); + } + else //right turn + { + dynamicsLeft.setSpeed(v1); + dynamicsRight.setSpeed(v2); + } + m_drivetrain->drive(dynamicsLeft, dynamicsRight); } units::Distance DriveArc::readAverageDistance() From 817742958daa43d1c2d61216dafebda55256beb1 Mon Sep 17 00:00:00 2001 From: solsane Date: Tue, 27 Feb 2018 13:41:10 -0500 Subject: [PATCH 013/200] makes navx a subsystem --- core/navigation/navx/CMakeLists.txt | 2 +- core/navigation/navx/include/navx/navx.hpp | 10 +++++++++- core/navigation/navx/src/navx.cpp | 15 +++++++++++++++ 3 files changed, 25 insertions(+), 2 deletions(-) diff --git a/core/navigation/navx/CMakeLists.txt b/core/navigation/navx/CMakeLists.txt index d25b510..8808b76 100644 --- a/core/navigation/navx/CMakeLists.txt +++ b/core/navigation/navx/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") endif() -target_link_libraries(${PROJECT_NAME} units spdlog periphery pthread) +target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread) target_include_directories(${PROJECT_NAME} PUBLIC include) if(ENABLE_DIAG) diff --git a/core/navigation/navx/include/navx/navx.hpp b/core/navigation/navx/include/navx/navx.hpp index e48b9de..c4ec415 100644 --- a/core/navigation/navx/include/navx/navx.hpp +++ b/core/navigation/navx/include/navx/navx.hpp @@ -7,6 +7,7 @@ #include #include #include +#include namespace rip { @@ -39,7 +40,7 @@ namespace rip */ using namespace rip; - class NavX + class NavX : public framework::Subsystem { public: @@ -748,6 +749,13 @@ namespace rip int getRequestedUpdateRate(); + /** + * Subsystem abstract Implementation + */ + virtual void stop() override; + + virtual bool diagnostic() override; + void close(); private: diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index d4ae21d..3974b4d 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -279,12 +279,16 @@ namespace rip }; NavX::NavX(std::string serial_port_id, NavX::serialDataType data_type, uint8_t update_rate_hz) + :Subsystem("") { + setName("navx"); serialInit(serial_port_id, data_type, update_rate_hz); } NavX::NavX(std::string serial_port_id) + :Subsystem("") { + setName("navx"); serialInit(serial_port_id, serialDataType::kProcessedData, NAVX_DEFAULT_UPDATE_RATE_HZ); } @@ -736,6 +740,17 @@ namespace rip { io->stop(); } + + void NavX::stop() + { + close(); + } + + bool NavX::diagnostic() + { + //todo + return 0; + } } } } From c6edea5b6916ee90ada07b3919274f3c5ca9cfd1 Mon Sep 17 00:00:00 2001 From: solsane Date: Tue, 27 Feb 2018 19:52:33 -0500 Subject: [PATCH 014/200] hotfix, fix navx getAngle --- core/navigation/navx/CMakeLists.txt | 2 +- core/navigation/navx/diag/diag_navx.cpp | 4 ++-- core/navigation/navx/src/navx.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/core/navigation/navx/CMakeLists.txt b/core/navigation/navx/CMakeLists.txt index 8808b76..6bf2a6c 100644 --- a/core/navigation/navx/CMakeLists.txt +++ b/core/navigation/navx/CMakeLists.txt @@ -14,7 +14,7 @@ endif() target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread) target_include_directories(${PROJECT_NAME} PUBLIC include) -if(ENABLE_DIAG) +if(ENABLE_TESTING) file(GLOB_RECURSE ${PROJECT_NAME}_DIAG_SOURCES "diag/*.cpp") add_executable(${PROJECT_NAME}_diag ${${PROJECT_NAME}_DIAG_SOURCES}) set_property(TARGET ${PROJECT_NAME}_diag PROPERTY CXX_STANDARD 11) diff --git a/core/navigation/navx/diag/diag_navx.cpp b/core/navigation/navx/diag/diag_navx.cpp index 40fe09d..d26d5fb 100644 --- a/core/navigation/navx/diag/diag_navx.cpp +++ b/core/navigation/navx/diag/diag_navx.cpp @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include #include #include @@ -31,7 +31,7 @@ int main(int argc, char *argv[]) { while(true) { std::cout << std::fixed << std::setprecision(2) << com.getPitch() << " " << com.getRoll(); - std::cout << " " << com.getYaw() << " " <displacement[1] = navx_update.disp_y; navx->displacement[2] = navx_update.disp_z; - navx->yaw_angle_tracker->nextAngle((navx->getYaw())()); + navx->yaw_angle_tracker->nextAngle((navx->getYaw()).to(units::deg)); navx->last_sensor_timestamp = sensor_timestamp; } From 8e1369e1e2040978e357a53c1ec54efc0de66425 Mon Sep 17 00:00:00 2001 From: solsane Date: Tue, 27 Feb 2018 19:53:53 -0500 Subject: [PATCH 015/200] changes to make turn action work --- .../navigation_actions/turn_to_angle.hpp | 4 ++-- core/navigation/actions/src/turn_to_angle.cpp | 6 ++++-- core/navigation/navx/include/navx/navx.hpp | 17 ++++++++--------- core/navigation/navx/src/navx.cpp | 17 ++++++++++------- 4 files changed, 24 insertions(+), 20 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index bcf664a..104fd13 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -33,7 +33,7 @@ namespace rip TurnToAngle(const std::string& name, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - std::shared_ptr navx, units::Distance& radius); + std::shared_ptr navx, const units::Distance& radius); /** * Returns whether or not the action has finished execution. * Returns true when navX reports change in angle that was @@ -57,7 +57,7 @@ namespace rip units::AngularVelocity m_speed; std::shared_ptr m_drivetrain; std::shared_ptr m_navx; - units::Angle m_desiredAngle; + units::Angle m_desiredAngle, m_currentAngle, m_init; units::Distance m_c2wRadius; }; } diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index cf87520..8fa005a 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -11,7 +11,7 @@ namespace rip TurnToAngle::TurnToAngle(const std::string& name, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - std::shared_ptr navx, units::Distance& radius) + std::shared_ptr navx, const units::Distance& radius) : Action(name) , m_drivetrain(drivetrain) , m_speed(speed) @@ -23,6 +23,8 @@ namespace rip bool TurnToAngle::isFinished() { units::Angle currentAngle = m_navx->getAngle(); + misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}", m_currentAngle.to(units::deg))); + //misc::Logger::getInstance()->debug(fmt::format("angular velocity: {}", m_navx->getRate().to(units::rev / units::minute))); if(m_desiredAngle >= 0) { @@ -51,7 +53,7 @@ namespace rip { m_speed *= -1; } - if(m_desiredAngle < 0) + if(m_desiredAngle() < 0) { m_speed *= -1; } diff --git a/core/navigation/navx/include/navx/navx.hpp b/core/navigation/navx/include/navx/navx.hpp index c4ec415..ca09db4 100644 --- a/core/navigation/navx/include/navx/navx.hpp +++ b/core/navigation/navx/include/navx/navx.hpp @@ -755,6 +755,14 @@ namespace rip virtual void stop() override; virtual bool diagnostic() override; + + /** + * Creates & returns a pointer to a navX object, constructed based on parameters + * inside json. Used in RIP subsystem creation + * @param config json parameters for navX construction + * @return shared ptr to navX object + */ + static std::shared_ptr makeNavX(const nlohmann::json& config); void close(); @@ -765,15 +773,6 @@ namespace rip uint8_t getActualUpdateRateInternal(uint8_t update_rate); }; - - /** - * Creates & returns a pointer to a navX object, constructed based on parameters - * inside json. Used in RIP subsystem creation - * @param config json parameters for navX construction - * @return shared ptr to navX object - */ - static std::shared_ptr makeNavX(const nlohmann::json& config); - } // navx } // navigation } // rip diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index 3974b4d..5826d7f 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -15,6 +15,7 @@ #include "navx/offset_tracker.hpp" #include "navx/continuous_angle_tracker.hpp" #include "navx/serial_io.hpp" + namespace rip { namespace navigation @@ -31,12 +32,6 @@ namespace rip static const uint8_t NAVX_MXP_I2C_ADDRESS = 0x32; static const float QUATERNION_HISTORY_SECONDS = 5.0f; - std::shared_ptr makeNavX(const nlohmann::json& config) - { - std::string device = config["device"]; - return std::make_shared(device); - } - class NavXInternal : public IIOCompleteNotification, public IBoardCapabilities { NavX* navx; @@ -745,12 +740,20 @@ namespace rip { close(); } - + bool NavX::diagnostic() { //todo return 0; } + + std::shared_ptr NavX::makeNavX(const nlohmann::json& config) + { + std::string device = config["device"]; + return std::make_shared(device); + } + + } } } From 1c8225e7238d5fe74a4d53cdd3ec6fe8a5622eac Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Feb 2018 23:32:28 -0500 Subject: [PATCH 016/200] Fix DriveArc contructor args --- core/navigation/actions/src/drive_arc.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 9c6227c..4f63fe0 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -9,7 +9,7 @@ namespace rip DriveArc::DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - units::Distance& radius, units::Distance& axleLength) + const units::Distance& radius, const units::Distance& axleLength) : Action(name) , m_direction(direction) , m_drivetrain(drivetrain) @@ -25,7 +25,7 @@ namespace rip DriveArc::DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, const units::Velocity& speed, const units::Distance& arcLength, - units::Distance& radius, units::Distance& axleLength) + const units::Distance& radius, const units::Distance& axleLength) : Action(name) , m_direction(direction) , m_drivetrain(drivetrain) From 875086cc1753935ef3b7c0b8f6c91dfe3230c753 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Feb 2018 23:40:26 -0500 Subject: [PATCH 017/200] Annddd the include --- .../actions/include/navigation_actions/drive_arc.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index 9fc4daa..9b9ca2b 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -32,7 +32,7 @@ namespace rip DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - units::Distance& radius, units::Distance& axleLength); + const units::Distance& radius, const units::Distance& axleLength); /** * Drives the robot in an arc with specified radius, arc length, @@ -48,7 +48,7 @@ namespace rip DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, const units::Velocity& speed, const units::Distance& arcLength, - units::Distance& radius, units::Distance& axleLength); + const units::Distance& radius, const units::Distance& axleLength); /** * Returns whether or not the action has finished execution. */ From 4d06d2fb16c7b7a22d37e3c7ab68acf3c44438ed Mon Sep 17 00:00:00 2001 From: "robo@blue-bot" Date: Wed, 28 Feb 2018 00:30:51 -0500 Subject: [PATCH 018/200] Fix missing space in cxx flags --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3fa2175..3fdfc17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,7 +8,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # to correctly compile the codebase despite this line being in the # downstream CMake files. set (CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS}-fdiagnostics-color=always") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) From 849b010f78ded844f53e5446f8b067c91ba025a7 Mon Sep 17 00:00:00 2001 From: solsane Date: Wed, 28 Feb 2018 09:32:58 -0500 Subject: [PATCH 019/200] cmake fixes, was having gcov linking issues on blu --- core/motor_controllers/CMakeLists.txt | 10 +++++----- core/navigation/navx/CMakeLists.txt | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/core/motor_controllers/CMakeLists.txt b/core/motor_controllers/CMakeLists.txt index 8f8d606..e11147a 100644 --- a/core/motor_controllers/CMakeLists.txt +++ b/core/motor_controllers/CMakeLists.txt @@ -19,7 +19,7 @@ if(ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}_diag motor_controllers periphery units) target_include_directories(${PROJECT_NAME}_diag PUBLIC diag) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage -lgcov --coverage") # Get test source files and test data files file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") @@ -37,8 +37,8 @@ if(ENABLE_TESTING) target_link_libraries(${PROJECT_NAME}_test gmock gtest) target_include_directories(${PROJECT_NAME}_test PUBLIC include test) - #if(CMAKE_COMPILER_IS_GNUCXX) - # include(CodeCoverage) - # setup_target_for_coverage(${PROJECT_NAME}_coverage ${PROJECT_NAME}_test coverage) - #endif() + if(CMAKE_COMPILER_IS_GNUCXX) + include(CodeCoverage) + setup_target_for_coverage(${PROJECT_NAME}_coverage ${PROJECT_NAME}_test coverage) + endif() endif() diff --git a/core/navigation/navx/CMakeLists.txt b/core/navigation/navx/CMakeLists.txt index 6bf2a6c..5fb0fe9 100644 --- a/core/navigation/navx/CMakeLists.txt +++ b/core/navigation/navx/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") endif() -target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread) +target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread gcov) target_include_directories(${PROJECT_NAME} PUBLIC include) if(ENABLE_TESTING) From 021b977cde7202c7909470a511ac08d053ca4491 Mon Sep 17 00:00:00 2001 From: Aiden Date: Wed, 28 Feb 2018 10:43:23 -0500 Subject: [PATCH 020/200] debugging statements --- core/navigation/navx/CMakeLists.txt | 2 +- core/navigation/navx/src/navx.cpp | 11 ++++++++--- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/core/navigation/navx/CMakeLists.txt b/core/navigation/navx/CMakeLists.txt index 5fb0fe9..23634d6 100644 --- a/core/navigation/navx/CMakeLists.txt +++ b/core/navigation/navx/CMakeLists.txt @@ -11,7 +11,7 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") endif() -target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread gcov) +target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread gcov misc) target_include_directories(${PROJECT_NAME} PUBLIC include) if(ENABLE_TESTING) diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index 70fb72b..eb8fb4d 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -15,6 +15,8 @@ #include "navx/offset_tracker.hpp" #include "navx/continuous_angle_tracker.hpp" #include "navx/serial_io.hpp" +#include +#include namespace rip { @@ -283,8 +285,11 @@ namespace rip NavX::NavX(std::string serial_port_id) :Subsystem("") { + setName("navx"); serialInit(serial_port_id, serialDataType::kProcessedData, NAVX_DEFAULT_UPDATE_RATE_HZ); + misc::Logger::getInstance()->debug(fmt::format("navx constructed, device: {}" + , serial_port_id)); } units::Angle NavX::getPitch() @@ -740,20 +745,20 @@ namespace rip { close(); } - + bool NavX::diagnostic() { //todo return 0; } - + std::shared_ptr NavX::makeNavX(const nlohmann::json& config) { std::string device = config["device"]; return std::make_shared(device); } - + } } } From 580baa227ac02e23347b7da8252a810418cd82d7 Mon Sep 17 00:00:00 2001 From: solsane Date: Wed, 28 Feb 2018 15:15:29 -0500 Subject: [PATCH 021/200] continuous angle tracker forgets crossings on zeroYaw --- .../navx/include/navx/continuous_angle_tracker.hpp | 1 + core/navigation/navx/src/continuous_angle_tracker.cpp | 5 +++++ core/navigation/navx/src/navx.cpp | 7 ++++--- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/core/navigation/navx/include/navx/continuous_angle_tracker.hpp b/core/navigation/navx/include/navx/continuous_angle_tracker.hpp index ec37e4a..e0b485e 100644 --- a/core/navigation/navx/include/navx/continuous_angle_tracker.hpp +++ b/core/navigation/navx/include/navx/continuous_angle_tracker.hpp @@ -25,6 +25,7 @@ namespace rip void nextAngle(float newAngle); double getAngle(); double getRate(); + void reset(); }; } } diff --git a/core/navigation/navx/src/continuous_angle_tracker.cpp b/core/navigation/navx/src/continuous_angle_tracker.cpp index d97a7eb..1209938 100644 --- a/core/navigation/navx/src/continuous_angle_tracker.cpp +++ b/core/navigation/navx/src/continuous_angle_tracker.cpp @@ -89,6 +89,11 @@ namespace rip { return this->last_rate; } + + void ContinuousAngleTracker::reset() + { + this->zero_crossing_count = 0; + } } } } diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index 70fb72b..56ff828 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -318,6 +318,7 @@ namespace rip { if (navx_internal->isBoardYawResetSupported()) { + yaw_angle_tracker->reset(); io->zeroYaw(); } else @@ -740,20 +741,20 @@ namespace rip { close(); } - + bool NavX::diagnostic() { //todo return 0; } - + std::shared_ptr NavX::makeNavX(const nlohmann::json& config) { std::string device = config["device"]; return std::make_shared(device); } - + } } } From 194c5425c519f57af928b9db1c3b667e9a22616c Mon Sep 17 00:00:00 2001 From: solsane Date: Thu, 1 Mar 2018 18:18:25 -0500 Subject: [PATCH 022/200] fixes --- .../include/navigation_actions/drive_arc.hpp | 4 +- .../include/navigation_actions/exceptions.hpp | 38 +++++++++++++++++++ .../navigation_actions/turn_to_angle.hpp | 2 +- core/navigation/actions/src/drive_arc.cpp | 4 +- core/navigation/actions/src/turn_to_angle.cpp | 24 +++++------- core/navigation/navx/diag/diag_navx.cpp | 4 +- .../path_planner/teb_planner/CMakeLists.txt | 2 +- 7 files changed, 56 insertions(+), 22 deletions(-) create mode 100644 core/navigation/actions/include/navigation_actions/exceptions.hpp diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index 9b9ca2b..b64e3a8 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -13,11 +13,11 @@ namespace rip { namespace actions { - using Motor = rip::navigation::drivetrains::Drivetrain::Motor; class DriveArc : public framework::Action { public: - + using Motor = rip::navigation::drivetrains::Drivetrain::Motor; + /** * Drives the robot in an arc with specified radius, angular distance, * angular velocity for the turn diff --git a/core/navigation/actions/include/navigation_actions/exceptions.hpp b/core/navigation/actions/include/navigation_actions/exceptions.hpp new file mode 100644 index 0000000..568aa1c --- /dev/null +++ b/core/navigation/actions/include/navigation_actions/exceptions.hpp @@ -0,0 +1,38 @@ +/* + * The RIP License (Revision 0.3): + * This software is available without warranty and without support. + * Use at your own risk. Literally. It might delete your filesystem or + * eat your cat. As long as you retain this notice, you can do whatever + * you want with this. If we meet some day, you owe me a beer. + * + * Go Vols! + * + * __ __ ________ __ __ _______ ______ _______ + * | \ | \| \| \ / \ | \ | \| \ + * | $$ | $$ \$$$$$$$$| $$ / $$ | $$$$$$$\ \$$$$$$| $$$$$$$\ + * | $$ | $$ | $$ | $$/ $$ | $$__| $$ | $$ | $$__/ $$ + * | $$ | $$ | $$ | $$ $$ | $$ $$ | $$ | $$ $$ + * | $$ | $$ | $$ | $$$$$\ | $$$$$$$\ | $$ | $$$$$$$ + * | $$__/ $$ | $$ | $$ \$$\ | $$ | $$ _| $$_ | $$ + * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ + * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ + */ +#ifndef EXCEPTIONS_HPP +#define EXCEPTIONS_HPP +#include + +namespace rip +{ + namespace navigation + { + namespace actions + { + /** + * Thrown if a parameter is out of bounds + * @param OutofBoundsException [description] + */ + NEW_EX(OutofBoundsException); + } + } +} +#endif // EXCEPTIONS_HPP diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index 104fd13..52f7d7e 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -57,7 +57,7 @@ namespace rip units::AngularVelocity m_speed; std::shared_ptr m_drivetrain; std::shared_ptr m_navx; - units::Angle m_desiredAngle, m_currentAngle, m_init; + units::Angle m_desiredAngle, m_priorAngle, m_init; units::Distance m_c2wRadius; }; } diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 4f63fe0..5afaa82 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -82,9 +82,9 @@ namespace rip units::Distance DriveArc::readAverageDistance() { + std::vector motors = {Motor::kFrontLeft, Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}; units::Distance sum=0; - std::vector dist = m_drivetrain->readEncoders({Motor::kFrontLeft, - Motor::kFrontLeft, Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}); + std::vector dist = m_drivetrain->readEncoders(motors); for(int i=0; i(dist.size()); i++) { sum += dist[i]; diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 8fa005a..6ec45d7 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -1,6 +1,7 @@ #include "navigation_actions/turn_to_angle.hpp" #include #include +#include namespace rip { @@ -23,17 +24,16 @@ namespace rip bool TurnToAngle::isFinished() { units::Angle currentAngle = m_navx->getAngle(); - misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}", m_currentAngle.to(units::deg))); - //misc::Logger::getInstance()->debug(fmt::format("angular velocity: {}", m_navx->getRate().to(units::rev / units::minute))); - if(m_desiredAngle >= 0) + if(m_priorAngle() != currentAngle()) { - return currentAngle >= m_desiredAngle; - } - else - { - return currentAngle <= m_desiredAngle; + misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" + , currentAngle.to(units::deg) - m_init.to(units::deg))); } + m_priorAngle = currentAngle; + + return std::abs(currentAngle.to(units::rad) - m_init.to(units::rad)) >= std::abs(m_desiredAngle.to(units::rad)); + } void TurnToAngle::update(nlohmann::json& state) @@ -44,18 +44,14 @@ namespace rip void TurnToAngle::setup(nlohmann::json& state) { //initial angle - misc::Logger::getInstance()->debug("setting yaw offset"); m_navx->zeroYaw(); motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" , m_speed.to(units::rev / units::minute))); - if(m_speed() < 0) - { - m_speed *= -1; - } + if(m_desiredAngle() < 0) { - m_speed *= -1; + throw OutofBoundsException("degrees should be positive. Sign of velocity determines turning direction"); } misc::Logger::getInstance()->debug(fmt::format("wheel linear speed (in/s): {}" , (m_speed * m_c2wRadius / units::rad).to(units::in / units::s))); diff --git a/core/navigation/navx/diag/diag_navx.cpp b/core/navigation/navx/diag/diag_navx.cpp index d26d5fb..2f261a7 100644 --- a/core/navigation/navx/diag/diag_navx.cpp +++ b/core/navigation/navx/diag/diag_navx.cpp @@ -24,7 +24,7 @@ int main(int argc, char *argv[]) { printf("Initializing\n\n"); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + //std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::cout << "Pitch | Roll | Yaw | X-Accel | Y-Accel | Z-Accel | Time |" << std::endl; @@ -34,7 +34,7 @@ int main(int argc, char *argv[]) { std::cout << " " << com.getAngle() << " " < Date: Thu, 1 Mar 2018 19:41:16 -0500 Subject: [PATCH 023/200] some changes for hugo, sloppy does trick for now --- core/navigation/actions/src/turn_to_angle.cpp | 15 +++++++++++---- core/navigation/navx/diag/diag_navx.cpp | 2 +- .../navx/src/continuous_angle_tracker.cpp | 3 +++ core/navigation/navx/src/navx.cpp | 5 +++-- 4 files changed, 18 insertions(+), 7 deletions(-) diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 6ec45d7..4838025 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -2,6 +2,8 @@ #include #include #include +#include +#include namespace rip { @@ -29,11 +31,13 @@ namespace rip { misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" , currentAngle.to(units::deg) - m_init.to(units::deg))); + misc::Logger::getInstance()->debug(fmt::format("current: {}" + , currentAngle.to(units::deg))); } m_priorAngle = currentAngle; - - return std::abs(currentAngle.to(units::rad) - m_init.to(units::rad)) >= std::abs(m_desiredAngle.to(units::rad)); - + misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" + , std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)))); + return std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)) >= std::abs(m_desiredAngle.to(units::deg)); } void TurnToAngle::update(nlohmann::json& state) @@ -44,7 +48,8 @@ namespace rip void TurnToAngle::setup(nlohmann::json& state) { //initial angle - m_navx->zeroYaw(); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + m_init = m_navx->getYaw(); motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" , m_speed.to(units::rev / units::minute))); @@ -55,6 +60,8 @@ namespace rip } misc::Logger::getInstance()->debug(fmt::format("wheel linear speed (in/s): {}" , (m_speed * m_c2wRadius / units::rad).to(units::in / units::s))); + misc::Logger::getInstance()->debug(fmt::format("init: {}" + , m_init.to(units::deg))); dynamicsLeft.setSpeed(m_speed * m_c2wRadius / units::rad); dynamicsRight.setSpeed(-1 * m_speed * m_c2wRadius / units::rad); diff --git a/core/navigation/navx/diag/diag_navx.cpp b/core/navigation/navx/diag/diag_navx.cpp index 2f261a7..3ccd410 100644 --- a/core/navigation/navx/diag/diag_navx.cpp +++ b/core/navigation/navx/diag/diag_navx.cpp @@ -24,7 +24,7 @@ int main(int argc, char *argv[]) { printf("Initializing\n\n"); - //std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::cout << "Pitch | Roll | Yaw | X-Accel | Y-Accel | Z-Accel | Time |" << std::endl; diff --git a/core/navigation/navx/src/continuous_angle_tracker.cpp b/core/navigation/navx/src/continuous_angle_tracker.cpp index 1209938..9a8dc11 100644 --- a/core/navigation/navx/src/continuous_angle_tracker.cpp +++ b/core/navigation/navx/src/continuous_angle_tracker.cpp @@ -93,6 +93,9 @@ namespace rip void ContinuousAngleTracker::reset() { this->zero_crossing_count = 0; + this->last_angle = 0.0f; + this->last_rate = 0; + this->first_sample = false; } } } diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index 61bd7a4..d7a5f4d 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -289,7 +289,7 @@ namespace rip setName("navx"); serialInit(serial_port_id, serialDataType::kProcessedData, NAVX_DEFAULT_UPDATE_RATE_HZ); misc::Logger::getInstance()->debug(fmt::format("navx constructed, device: {}" - , serial_port_id)); + , serial_port_id)); } units::Angle NavX::getPitch() @@ -323,11 +323,12 @@ namespace rip { if (navx_internal->isBoardYawResetSupported()) { - yaw_angle_tracker->reset(); io->zeroYaw(); + yaw_angle_tracker->reset(); } else { + yaw_angle_tracker->reset(); yaw_offset_tracker->setOffset(); } } From a9e663afd0eac035605fe0785e588a734df29a9c Mon Sep 17 00:00:00 2001 From: solsane Date: Fri, 2 Mar 2018 15:44:56 -0500 Subject: [PATCH 024/200] arc turn works --- core/navigation/actions/src/drive_arc.cpp | 15 ++++++++++++++- core/navigation/actions/src/turn_to_angle.cpp | 13 +++++-------- .../drivetrains/src/two_roboclaw_drivetrain.cpp | 17 +++++++++++++---- 3 files changed, 32 insertions(+), 13 deletions(-) diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 5afaa82..9ac4dbc 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -41,7 +41,8 @@ namespace rip bool DriveArc::isFinished() { m_traveled = readAverageDistance(); - return m_arcLength >= (m_traveled - m_init); + + return m_arcLength <= (m_traveled - m_init); } void DriveArc::update(nlohmann::json& state) @@ -55,6 +56,8 @@ namespace rip std::vector dist; units::Velocity v1, v2; m_init = readAverageDistance(); + misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear velocity(in/s): {}" + , m_linearSpeed.to(units::in / units::s))); misc::Logger::getInstance()->debug(fmt::format("initial(offset) distance(ft): {}", m_init.to(units::ft))); motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; @@ -65,8 +68,18 @@ namespace rip misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear velocity(in/s): {}" , m_linearSpeed.to(units::in / units::s))); + misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear distance (in): {}" + , m_arcLength.to(units::in))); + v1 = m_angularSpeed * (m_radius + m_axleLength/2 ) / units::rad; v2 = m_angularSpeed * (m_radius - m_axleLength/2 ) / units::rad; + + misc::Logger::getInstance()->debug(fmt::format("arc turn outer motor linear velocity (in/s): {}" + , v1.to(units::in / units::s))); + + misc::Logger::getInstance()->debug(fmt::format("arc turn outer motor linear velocity (in/s): {}" + , v2.to(units::in / units::s))); + if(!m_direction) // left turn { dynamicsLeft.setSpeed(v2); diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 4838025..1fe7f94 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -2,8 +2,6 @@ #include #include #include -#include -#include namespace rip { @@ -27,16 +25,16 @@ namespace rip { units::Angle currentAngle = m_navx->getAngle(); - if(m_priorAngle() != currentAngle()) + /*if(m_priorAngle() != currentAngle()) { misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" , currentAngle.to(units::deg) - m_init.to(units::deg))); misc::Logger::getInstance()->debug(fmt::format("current: {}" , currentAngle.to(units::deg))); - } + }*/ m_priorAngle = currentAngle; - misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" - , std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)))); + //misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" + //, std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)))); return std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)) >= std::abs(m_desiredAngle.to(units::deg)); } @@ -48,8 +46,7 @@ namespace rip void TurnToAngle::setup(nlohmann::json& state) { //initial angle - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - m_init = m_navx->getYaw(); + m_init = m_navx->getAngle(); motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" , m_speed.to(units::rev / units::minute))); diff --git a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp index d426576..fdf0c82 100644 --- a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp +++ b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp @@ -2,6 +2,8 @@ #include #include #include +#include +#include namespace rip { @@ -86,29 +88,36 @@ namespace rip std::vector TwoRoboclawDrivetrain::readEncoders(const std::vector& motors) { std::vector data; - for(int i=0; i(motors[i]) << std::endl; + switch(motors[i]) { + case Motor::kFrontLeft: { data.push_back(m_left->readEncoder(Roboclaw::Motor::kM1)); + break; } case Motor::kFrontRight: { data.push_back(m_right->readEncoder(Roboclaw::Motor::kM1)); + break; } case Motor::kBackLeft: { data.push_back(m_left->readEncoder(Roboclaw::Motor::kM2)); + break; } case Motor::kBackRight: { data.push_back(m_right->readEncoder(Roboclaw::Motor::kM2)); + break; } default: { - throw InvalidMotorException("Invalid motor"); + throw InvalidMotorException(fmt::format("Invalid motor, parameter {}", i+1)); } } } @@ -146,7 +155,7 @@ namespace rip const std::vector& motors) { std::vector data; - for(int i=0; i Date: Sun, 4 Mar 2018 14:03:37 -0500 Subject: [PATCH 025/200] minor changes suggested on PR' --- core/navigation/navx/CMakeLists.txt | 6 ++++-- core/navigation/navx/diag/diag_navx.cpp | 13 ++++++------- .../path_planner/teb_planner/CMakeLists.txt | 6 ++++-- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/core/navigation/navx/CMakeLists.txt b/core/navigation/navx/CMakeLists.txt index 23634d6..f97edd0 100644 --- a/core/navigation/navx/CMakeLists.txt +++ b/core/navigation/navx/CMakeLists.txt @@ -11,9 +11,11 @@ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) if(ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") endif() -target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread gcov misc) +target_link_libraries(${PROJECT_NAME} framework units spdlog periphery pthread misc) target_include_directories(${PROJECT_NAME} PUBLIC include) - +if(ENABLE_TESTING) + target_link_libraries(${PROJECT_NAME} gcov) +endif() if(ENABLE_TESTING) file(GLOB_RECURSE ${PROJECT_NAME}_DIAG_SOURCES "diag/*.cpp") add_executable(${PROJECT_NAME}_diag ${${PROJECT_NAME}_DIAG_SOURCES}) diff --git a/core/navigation/navx/diag/diag_navx.cpp b/core/navigation/navx/diag/diag_navx.cpp index 3ccd410..49711b1 100644 --- a/core/navigation/navx/diag/diag_navx.cpp +++ b/core/navigation/navx/diag/diag_navx.cpp @@ -6,24 +6,23 @@ #include #include #include +#include +#include volatile sig_atomic_t sflag = 0; using namespace rip::navigation::navx; + void handle_sig(int sig) { sflag = 1; } - -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ std::cout << "Program Executing\n"; signal(SIGINT, handle_sig); NavX com = NavX("/dev/ttyACM0"); - - printf("Initializing\n\n"); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); std::cout << "Pitch | Roll | Yaw | X-Accel | Y-Accel | Z-Accel | Time |" << std::endl; @@ -43,7 +42,7 @@ int main(int argc, char *argv[]) { break; } } - printf("\nExit Caught... Closing device.\n"); + std::cout("\nExit Caught... Closing device.\n"); return 0; } diff --git a/core/navigation/path_planner/teb_planner/CMakeLists.txt b/core/navigation/path_planner/teb_planner/CMakeLists.txt index c45a8b0..9205343 100644 --- a/core/navigation/path_planner/teb_planner/CMakeLists.txt +++ b/core/navigation/path_planner/teb_planner/CMakeLists.txt @@ -12,8 +12,10 @@ file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cpp) file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/teb_planner/*.hpp) add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -target_link_libraries(${PROJECT_NAME} fmt gcov json optional units geometry misc ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} fmt json optional units geometry misc ${SUITESPARSE_LIBRARIES} ${G2O_LIBRARIES}) target_include_directories(${PROJECT_NAME} PUBLIC include ${SUITESPARSE_INCLUDE_DIRS}) - +if(ENABLE_TESTING) + target_link_libraries(${PROJECT_NAME} gcov) +endif() message("CSPARSE INC: ${CSPARSE_INCLUDE_DIRS}") message("SUITESPARSE INC: ${SUITESPARSE_INCLUDE_DIRS}") From c44eeef387c4125d10246c4c5e004a7271cecaf3 Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 4 Mar 2018 15:45:02 -0500 Subject: [PATCH 026/200] changes to drivearc, turntoangle --- .../include/navigation_actions/drive_arc.hpp | 41 ++++++++--- .../navigation_actions/turn_to_angle.hpp | 13 ++++ core/navigation/actions/src/drive_arc.cpp | 71 ++++++++++++++----- core/navigation/actions/src/turn_to_angle.cpp | 16 ++--- core/navigation/navx/diag/diag_navx.cpp | 4 +- core/navigation/navx/src/navx.cpp | 7 +- 6 files changed, 105 insertions(+), 47 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index b64e3a8..b3962ed 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -6,6 +6,8 @@ #include #include #include +#include +#include "exceptions.hpp" namespace rip { @@ -13,11 +15,12 @@ namespace rip { namespace actions { + using NavX = navx::NavX; class DriveArc : public framework::Action { public: using Motor = rip::navigation::drivetrains::Drivetrain::Motor; - + /** * Drives the robot in an arc with specified radius, angular distance, * angular velocity for the turn @@ -27,12 +30,12 @@ namespace rip * @param speed angular velocity for the turn * @param angle angle to tunr * @param radius radius size of arc turn - * @param axleLength width between left and right wheels. + * @param axle_length width between left and right wheels. */ DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - const units::Distance& radius, const units::Distance& axleLength); + const units::Distance& radius, const units::Distance& axle_length); /** * Drives the robot in an arc with specified radius, arc length, @@ -41,14 +44,31 @@ namespace rip * @param direction 0 means left turn. 1 means right turn * @param drivetrain pointer to differential drivetrain. * @param speed angular velocity for the turn - * @param arcLength Linear distance that for the robot to travel + * @param arc_length Linear distance that for the robot to travel * @param radius radius size of arc turn - * @param axleLength width between left and right wheels. + * @param axle_length width between left and right wheels. + */ + DriveArc(const std::string& name, bool direction, + std::shared_ptr drivetrain, + const units::Velocity& speed, const units::Distance& arc_length, + const units::Distance& radius, const units::Distance& axle_length); + /** + * drivearc. but with a navx. + * @param name action name + * @param direction direction to turn + * @param drivetrain drivetrain + * @param speed linear speed (units) + * @param arc_length linear distance to drive + * @param radius radius to turn around + * @param axle_length length of axle base + * @param navx ptr to navx */ DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, - const units::Velocity& speed, const units::Distance& arcLength, - const units::Distance& radius, const units::Distance& axleLength); + const units::Velocity& speed, const units::Distance& arc_length, + const units::Distance& radius, const units::Distance& axle_length, + std::shared_ptr navx); + /** * Returns whether or not the action has finished execution. */ @@ -75,14 +95,15 @@ namespace rip private: bool m_direction; std::shared_ptr m_drivetrain; - units::AngularVelocity m_angularSpeed; - units::Velocity m_linearSpeed; + units::AngularVelocity m_angular_speed; + units::Velocity m_linear_speed; units::Angle m_angle; + std::shared_ptr m_navx; /** * radius is distance from center of robot/differential drivetrain * to center of circle of arc (ICC) */ - units::Distance m_arcLength, m_radius, m_axleLength, m_traveled, m_init; + units::Distance m_arc_length, m_radius, m_axle_length, m_traveled, m_init; }; } } diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index 52f7d7e..f165d60 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -34,6 +34,19 @@ namespace rip std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, std::shared_ptr navx, const units::Distance& radius); + /** + * encoder based turn to angle + * @param name action name + * @param drivetrain drivetrain + * @param speed angular velocity of turn + * @param angle how far to turn + * @param radius half of axle length + */ + TurnToAngle(const std::string& name, + std::shared_ptr drivetrain, + const units::AngularVelocity& speed, + const units::Angle& angle, + const units::Distance& radius); /** * Returns whether or not the action has finished execution. * Returns true when navX reports change in angle that was diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 9ac4dbc..b6b8c60 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -9,40 +9,58 @@ namespace rip DriveArc::DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - const units::Distance& radius, const units::Distance& axleLength) + const units::Distance& radius, const units::Distance& axle_length) : Action(name) , m_direction(direction) , m_drivetrain(drivetrain) - , m_angularSpeed(speed) + , m_angular_speed(speed) , m_angle(angle) , m_radius(radius) - , m_axleLength(axleLength) + , m_axle_length(axle_length) { - m_arcLength = angle.to(units::rad) * radius; - m_linearSpeed = radius * speed / units::rad; + m_arc_length = angle.to(units::rad) * radius; + m_linear_speed = radius * speed / units::rad; } DriveArc::DriveArc(const std::string& name, bool direction, std::shared_ptr drivetrain, - const units::Velocity& speed, const units::Distance& arcLength, - const units::Distance& radius, const units::Distance& axleLength) + const units::Velocity& speed, const units::Distance& arc_length, + const units::Distance& radius, const units::Distance& axle_length) : Action(name) , m_direction(direction) , m_drivetrain(drivetrain) - , m_linearSpeed(speed) - , m_arcLength(arcLength) + , m_linear_speed(speed) + , m_arc_length(arc_length) , m_radius(radius) - , m_axleLength(axleLength) + , m_axle_length(axle_length) { - m_angle = arcLength / radius * units::rad; - m_angularSpeed = speed / radius * units::rad; + m_angle = arc_length / radius * units::rad; + m_angular_speed = speed / radius * units::rad; + } + + DriveArc::DriveArc(const std::string& name, bool direction, + std::shared_ptr drivetrain, + const units::Velocity& speed, const units::Distance& arc_length, + const units::Distance& radius, const units::Distance& axle_length, + std::shared_ptr navx) + : Action(name) + , m_direction(direction) + , m_drivetrain(drivetrain) + , m_linear_speed(speed) + , m_arc_length(arc_length) + , m_radius(radius) + , m_axle_length(axle_length) + , m_navx(navx) + { + m_angle = arc_length / radius * units::rad; + m_angular_speed = speed / radius * units::rad; } bool DriveArc::isFinished() { m_traveled = readAverageDistance(); - return m_arcLength <= (m_traveled - m_init); + return m_arc_length <= (m_traveled - m_init); } void DriveArc::update(nlohmann::json& state) @@ -55,24 +73,39 @@ namespace rip { std::vector dist; units::Velocity v1, v2; + + if(m_arc_length <= 0) + { + throw OutofBoundsException("arc_length should be greater than 0"); + } + if(m_radius < m_axle_length) + { + throw OutofBoundsException("radius should be greater than the axle length"); + } + if(m_axle_length <= 0) + { + throw OutofBoundsException("axle length should be positive"); + } + m_init = readAverageDistance(); + misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear velocity(in/s): {}" - , m_linearSpeed.to(units::in / units::s))); + , m_linear_speed.to(units::in / units::s))); misc::Logger::getInstance()->debug(fmt::format("initial(offset) distance(ft): {}", m_init.to(units::ft))); motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; misc::Logger::getInstance()->debug(fmt::format("arc turn intended angular velocity (rev/min): {}" - , m_angularSpeed.to(units::rev / units::minute))); + , m_angular_speed.to(units::rev / units::minute))); misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear velocity(in/s): {}" - , m_linearSpeed.to(units::in / units::s))); + , m_linear_speed.to(units::in / units::s))); misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear distance (in): {}" - , m_arcLength.to(units::in))); + , m_arc_length.to(units::in))); - v1 = m_angularSpeed * (m_radius + m_axleLength/2 ) / units::rad; - v2 = m_angularSpeed * (m_radius - m_axleLength/2 ) / units::rad; + v1 = m_angular_speed * (m_radius + m_axle_length/2 ) / units::rad; + v2 = m_angular_speed * (m_radius - m_axle_length/2 ) / units::rad; misc::Logger::getInstance()->debug(fmt::format("arc turn outer motor linear velocity (in/s): {}" , v1.to(units::in / units::s))); diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 1fe7f94..8886f6d 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -24,17 +24,7 @@ namespace rip bool TurnToAngle::isFinished() { units::Angle currentAngle = m_navx->getAngle(); - - /*if(m_priorAngle() != currentAngle()) - { - misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" - , currentAngle.to(units::deg) - m_init.to(units::deg))); - misc::Logger::getInstance()->debug(fmt::format("current: {}" - , currentAngle.to(units::deg))); - }*/ m_priorAngle = currentAngle; - //misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" - //, std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)))); return std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)) >= std::abs(m_desiredAngle.to(units::deg)); } @@ -51,10 +41,14 @@ namespace rip misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" , m_speed.to(units::rev / units::minute))); - if(m_desiredAngle() < 0) + if(m_desiredAngle() <= 0) { throw OutofBoundsException("degrees should be positive. Sign of velocity determines turning direction"); } + if(m_c2wRadius <= 0) + { + throw OutofBoundsException("wheel radius should be positive"); + } misc::Logger::getInstance()->debug(fmt::format("wheel linear speed (in/s): {}" , (m_speed * m_c2wRadius / units::rad).to(units::in / units::s))); misc::Logger::getInstance()->debug(fmt::format("init: {}" diff --git a/core/navigation/navx/diag/diag_navx.cpp b/core/navigation/navx/diag/diag_navx.cpp index 49711b1..c9f42e2 100644 --- a/core/navigation/navx/diag/diag_navx.cpp +++ b/core/navigation/navx/diag/diag_navx.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include volatile sig_atomic_t sflag = 0; @@ -42,7 +42,7 @@ int main(int argc, char *argv[]) break; } } - std::cout("\nExit Caught... Closing device.\n"); + std::cout << "\nExit Caught... Closing device.\n"; return 0; } diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index d7a5f4d..52b8c9b 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -276,17 +276,14 @@ namespace rip }; NavX::NavX(std::string serial_port_id, NavX::serialDataType data_type, uint8_t update_rate_hz) - :Subsystem("") + :Subsystem("navx") { - setName("navx"); serialInit(serial_port_id, data_type, update_rate_hz); } NavX::NavX(std::string serial_port_id) - :Subsystem("") + :Subsystem("navx") { - - setName("navx"); serialInit(serial_port_id, serialDataType::kProcessedData, NAVX_DEFAULT_UPDATE_RATE_HZ); misc::Logger::getInstance()->debug(fmt::format("navx constructed, device: {}" , serial_port_id)); From e4de555b3907d36d805006e2823328fc1b3274fc Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 4 Mar 2018 19:45:40 -0500 Subject: [PATCH 027/200] changes to turn to angle. should work now --- core/navigation/actions/src/turn_to_angle.cpp | 9 +++++++++ core/navigation/navx/src/navx.cpp | 4 ++-- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index 8886f6d..a135a2f 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -25,6 +25,8 @@ namespace rip { units::Angle currentAngle = m_navx->getAngle(); m_priorAngle = currentAngle; + misc::Logger::getInstance()->debug(fmt::format("current angle: {}" + , currentAngle.to(units::deg))); return std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)) >= std::abs(m_desiredAngle.to(units::deg)); } @@ -36,7 +38,12 @@ namespace rip void TurnToAngle::setup(nlohmann::json& state) { //initial angle + m_init = m_navx->getAngle(); + do + { + m_init = m_navx->getAngle(); + } while(m_init.to(units::deg) == 0); motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" , m_speed.to(units::rev / units::minute))); @@ -61,6 +68,8 @@ namespace rip void TurnToAngle::teardown(nlohmann::json& state) { + misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" + , (m_priorAngle - m_init).to(units::deg))); m_drivetrain->stop(); } } diff --git a/core/navigation/navx/src/navx.cpp b/core/navigation/navx/src/navx.cpp index 52b8c9b..395e039 100644 --- a/core/navigation/navx/src/navx.cpp +++ b/core/navigation/navx/src/navx.cpp @@ -321,11 +321,11 @@ namespace rip if (navx_internal->isBoardYawResetSupported()) { io->zeroYaw(); - yaw_angle_tracker->reset(); + //yaw_angle_tracker->reset(); } else { - yaw_angle_tracker->reset(); + //yaw_angle_tracker->reset(); yaw_offset_tracker->setOffset(); } } From 97242b52bfebeb755de83b4758fd381c06ee6a5d Mon Sep 17 00:00:00 2001 From: solsane Date: Mon, 5 Mar 2018 09:14:32 -0500 Subject: [PATCH 028/200] camelCase vars -> snake_case --- .../navigation_actions/turn_to_angle.hpp | 4 ++-- core/navigation/actions/src/turn_to_angle.cpp | 24 +++++++++---------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp index f165d60..cee534a 100644 --- a/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp +++ b/core/navigation/actions/include/navigation_actions/turn_to_angle.hpp @@ -70,8 +70,8 @@ namespace rip units::AngularVelocity m_speed; std::shared_ptr m_drivetrain; std::shared_ptr m_navx; - units::Angle m_desiredAngle, m_priorAngle, m_init; - units::Distance m_c2wRadius; + units::Angle m_desired_angle, m_prior_angle, m_init; + units::Distance m_c2w_radius; }; } } diff --git a/core/navigation/actions/src/turn_to_angle.cpp b/core/navigation/actions/src/turn_to_angle.cpp index a135a2f..0b2a0f7 100644 --- a/core/navigation/actions/src/turn_to_angle.cpp +++ b/core/navigation/actions/src/turn_to_angle.cpp @@ -16,18 +16,18 @@ namespace rip : Action(name) , m_drivetrain(drivetrain) , m_speed(speed) - , m_desiredAngle(angle) + , m_desired_angle(angle) , m_navx(navx) - , m_c2wRadius(radius) + , m_c2w_radius(radius) {} bool TurnToAngle::isFinished() { - units::Angle currentAngle = m_navx->getAngle(); - m_priorAngle = currentAngle; + units::Angle current_angle = m_navx->getAngle(); + m_prior_angle = current_angle; misc::Logger::getInstance()->debug(fmt::format("current angle: {}" - , currentAngle.to(units::deg))); - return std::abs(currentAngle.to(units::deg) - m_init.to(units::deg)) >= std::abs(m_desiredAngle.to(units::deg)); + , current_angle.to(units::deg))); + return std::abs(current_angle.to(units::deg) - m_init.to(units::deg)) >= std::abs(m_desired_angle.to(units::deg)); } void TurnToAngle::update(nlohmann::json& state) @@ -48,28 +48,28 @@ namespace rip misc::Logger::getInstance()->debug(fmt::format("in place turn intended angular velocity (rev/min): {}" , m_speed.to(units::rev / units::minute))); - if(m_desiredAngle() <= 0) + if(m_desired_angle() <= 0) { throw OutofBoundsException("degrees should be positive. Sign of velocity determines turning direction"); } - if(m_c2wRadius <= 0) + if(m_c2w_radius <= 0) { throw OutofBoundsException("wheel radius should be positive"); } misc::Logger::getInstance()->debug(fmt::format("wheel linear speed (in/s): {}" - , (m_speed * m_c2wRadius / units::rad).to(units::in / units::s))); + , (m_speed * m_c2w_radius / units::rad).to(units::in / units::s))); misc::Logger::getInstance()->debug(fmt::format("init: {}" , m_init.to(units::deg))); - dynamicsLeft.setSpeed(m_speed * m_c2wRadius / units::rad); - dynamicsRight.setSpeed(-1 * m_speed * m_c2wRadius / units::rad); + dynamicsLeft.setSpeed(m_speed * m_c2w_radius / units::rad); + dynamicsRight.setSpeed(-1 * m_speed * m_c2w_radius / units::rad); m_drivetrain->drive(dynamicsLeft, dynamicsRight); } void TurnToAngle::teardown(nlohmann::json& state) { misc::Logger::getInstance()->debug(fmt::format("degrees turned: {}" - , (m_priorAngle - m_init).to(units::deg))); + , (m_prior_angle - m_init).to(units::deg))); m_drivetrain->stop(); } } From 5dd92d5a4269958068e3be2c80310e853690b71f Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Tue, 6 Mar 2018 17:20:31 -0500 Subject: [PATCH 029/200] undefined functions --- core/utilities/pid/src/linear_digital_filter.cpp | 6 ++++++ core/utilities/pid/src/pid.cpp | 5 +++++ 2 files changed, 11 insertions(+) diff --git a/core/utilities/pid/src/linear_digital_filter.cpp b/core/utilities/pid/src/linear_digital_filter.cpp index fad5420..f291467 100644 --- a/core/utilities/pid/src/linear_digital_filter.cpp +++ b/core/utilities/pid/src/linear_digital_filter.cpp @@ -59,5 +59,11 @@ namespace rip return rv; } + + void LinearDigitalFilter::reset() + { + m_inputs.reset(); + m_outputs.reset(); + } } } diff --git a/core/utilities/pid/src/pid.cpp b/core/utilities/pid/src/pid.cpp index dd90ba8..15be6c5 100644 --- a/core/utilities/pid/src/pid.cpp +++ b/core/utilities/pid/src/pid.cpp @@ -67,6 +67,11 @@ namespace rip return (m_setpoint - m_previous_setpoint) / dt.count(); } + double PidController::error() const + { + return m_error; + } + void PidController::setSetpoint(double setpoint) { m_setpoint = clamp(setpoint, m_minimum_input, m_maximum_input); From 7c396e72884a1232a7c646aaa432bd2e43637074 Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Tue, 6 Mar 2018 18:12:49 -0500 Subject: [PATCH 030/200] PID logging --- core/utilities/pid/CMakeLists.txt | 2 +- core/utilities/pid/src/pid.cpp | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/core/utilities/pid/CMakeLists.txt b/core/utilities/pid/CMakeLists.txt index 68babac..8dd670a 100644 --- a/core/utilities/pid/CMakeLists.txt +++ b/core/utilities/pid/CMakeLists.txt @@ -8,5 +8,5 @@ file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/*.cpp) file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/pid/*.hpp) add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -target_link_libraries(${PROJECT_NAME} units) +target_link_libraries(${PROJECT_NAME} units misc) target_include_directories(${PROJECT_NAME} PUBLIC include) diff --git a/core/utilities/pid/src/pid.cpp b/core/utilities/pid/src/pid.cpp index 15be6c5..93e3eca 100644 --- a/core/utilities/pid/src/pid.cpp +++ b/core/utilities/pid/src/pid.cpp @@ -1,5 +1,7 @@ #include "pid/pid.hpp" +#include + #include namespace rip @@ -171,6 +173,8 @@ namespace rip std::chrono::duration dt = time_now - m_last_time; double input = m_input->get(); + misc::Logger::getInstance()->debug("Input: {}", input); + PidInput::Type type = m_input->type(); // Storage for function inputs @@ -199,6 +203,7 @@ namespace rip } m_result = clamp(m_result, m_minimum_output, m_maximum_output); + misc::Logger::getInstance()->debug("Result: {}", m_result); if (m_enabled) { m_output->set(m_result); From d965f42032b6112e4393e702069ba7dba23e02de Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Tue, 6 Mar 2018 18:29:36 -0500 Subject: [PATCH 031/200] fixed null --- core/navigation/actions/src/drive_straight.cpp | 6 ++++-- core/utilities/pid/src/pid.cpp | 2 ++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/core/navigation/actions/src/drive_straight.cpp b/core/navigation/actions/src/drive_straight.cpp index 11dcbd0..0a0b63f 100644 --- a/core/navigation/actions/src/drive_straight.cpp +++ b/core/navigation/actions/src/drive_straight.cpp @@ -13,7 +13,8 @@ namespace rip , m_distance(distance) , m_speed(speed) , m_drivetrain(drivetrain) - , m_pid(new pid::PidController(m_navx.get(), this, p, i , d)) + , m_navx(navx) + , m_pid(new pid::PidController(navx.get(), this, p, i , d)) {} DriveStraight::DriveStraight(const std::string& name, std::shared_ptr drivetrain, std::shared_ptr navx, @@ -23,7 +24,8 @@ namespace rip , m_time(time) , m_speed(speed) , m_drivetrain(drivetrain) - , m_pid(new pid::PidController(m_navx.get(), this, p, i , d)) + , m_navx(navx) + , m_pid(new pid::PidController(navx.get(), this, p, i , d)) {} bool DriveStraight::isFinished() diff --git a/core/utilities/pid/src/pid.cpp b/core/utilities/pid/src/pid.cpp index 93e3eca..0d15978 100644 --- a/core/utilities/pid/src/pid.cpp +++ b/core/utilities/pid/src/pid.cpp @@ -90,6 +90,8 @@ namespace rip return std::fabs(err) < m_tolerance; case ToleranceType::kNoTolerance: return false; + default: + assert(false); } return false; } From a709499f3ff1ba2a3a774afe9eb7c913c8471a96 Mon Sep 17 00:00:00 2001 From: Ben Johnson Date: Tue, 6 Mar 2018 19:13:17 -0500 Subject: [PATCH 032/200] servo files updated --- appendages/json/servo.json | 3 +++ appendages/src/servo.cpp | 8 ++++++-- appendages/xml/servo.xml | 33 +++++++++++++++++++++++++++++++++ 3 files changed, 42 insertions(+), 2 deletions(-) create mode 100644 appendages/json/servo.json create mode 100644 appendages/xml/servo.xml diff --git a/appendages/json/servo.json b/appendages/json/servo.json new file mode 100644 index 0000000..8bd4e95 --- /dev/null +++ b/appendages/json/servo.json @@ -0,0 +1,3 @@ +{ + "pin": "int" +} diff --git a/appendages/src/servo.cpp b/appendages/src/servo.cpp index e20d711..9e2ec3d 100644 --- a/appendages/src/servo.cpp +++ b/appendages/src/servo.cpp @@ -6,13 +6,16 @@ #include +using namespace rip::utilities; + namespace rip { namespace appendages { Servo::Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device) - , m_write(createCommand("kServoWrite", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_write(createCommand("kServoWrite", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_id(device) // MAYBE? { } @@ -24,11 +27,12 @@ namespace rip void Servo::stop() { - + // no stop command? } bool Servo::diagnostic() { + // TODO figure out what this is return true; } diff --git a/appendages/xml/servo.xml b/appendages/xml/servo.xml new file mode 100644 index 0000000..a9bc2c2 --- /dev/null +++ b/appendages/xml/servo.xml @@ -0,0 +1,33 @@ + + + Servo.h + + + + + + + + + + + + servos[$i$].attach($pin$) + + + + // Servo pin: $pin$ + + + + + + + if (!servos[indexNum].attached()) { + servos[indexNum].write(value) + } + servos[indexNum].write(value) + + + + From 34218385ce594e881b91239e30894454bfb819a9 Mon Sep 17 00:00:00 2001 From: Ben Johnson Date: Tue, 6 Mar 2018 19:30:30 -0500 Subject: [PATCH 033/200] digital_input.xml: fixed command name --- appendages/xml/digital_input.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/appendages/xml/digital_input.xml b/appendages/xml/digital_input.xml index 913c199..3fb2341 100644 --- a/appendages/xml/digital_input.xml +++ b/appendages/xml/digital_input.xml @@ -8,7 +8,7 @@ - + rv = digitalRead(digital_input_pins[index_num]); From 4db3bbabb81219b7bde315367b01002bdc30531a Mon Sep 17 00:00:00 2001 From: Ben Johnson Date: Tue, 6 Mar 2018 19:49:54 -0500 Subject: [PATCH 034/200] removed m_id() and added appendage register --- appendages/src/appendage_factory.cpp | 1 + appendages/src/servo.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index ae785d3..55e6aa7 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -45,6 +45,7 @@ namespace rip { registerAppendage("Digital Input", &DigitalInput::create); registerAppendage("Analog Input", &AnalogInput::create); + registerAppendage("Servo", &DigitalInput::create); } } } diff --git a/appendages/src/servo.cpp b/appendages/src/servo.cpp index 9e2ec3d..be488ad 100644 --- a/appendages/src/servo.cpp +++ b/appendages/src/servo.cpp @@ -15,7 +15,6 @@ namespace rip Servo::Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device) , m_write(createCommand("kServoWrite", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) - , m_id(device) // MAYBE? { } From 8231bb029241e767376cf3895d1922cc3f2e6ff3 Mon Sep 17 00:00:00 2001 From: Andrew Date: Tue, 6 Mar 2018 20:35:38 -0500 Subject: [PATCH 035/200] actions --- core/motor_controllers/src/roboclaw.cpp | 8 ++++---- core/navigation/actions/src/drive_straight.cpp | 14 +++++++++++--- core/utilities/pid/src/linear_digital_filter.cpp | 2 +- core/utilities/pid/src/pid.cpp | 15 +++++++++++++-- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index 7abbe4d..6cbe1d1 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -713,21 +713,21 @@ namespace rip data = read(&m_serial, m_timeout); crcUpdate(data); response.push_back(data); - if (data == -1) + if (data == 0xff) { continue; } } - if (data != -1) + if (data != 0xff) { uint16_t ccrc; data = read(&m_serial, m_timeout); - if (data != -1) + if (data != 0xff) { ccrc = static_cast(data) << 8; data = read(&m_serial, m_timeout); - if (data != -1) + if (data != 0xff) { ccrc |= data; if (crcGet() == ccrc) diff --git a/core/navigation/actions/src/drive_straight.cpp b/core/navigation/actions/src/drive_straight.cpp index 0a0b63f..63e25ae 100644 --- a/core/navigation/actions/src/drive_straight.cpp +++ b/core/navigation/actions/src/drive_straight.cpp @@ -26,7 +26,14 @@ namespace rip , m_drivetrain(drivetrain) , m_navx(navx) , m_pid(new pid::PidController(navx.get(), this, p, i , d)) - {} + { + m_pid->setSetpoint(0); + m_pid->setTolerance(10); + m_navx->setType(pid::PidInput::Type::kDisplacement); + m_pid->setContinuous(true); + m_pid->setInputRange(-180 * units::deg(), 180 * units::deg()); + m_pid->setOutputRange(-2 * units::in(), 2 * units::in()); + } bool DriveStraight::isFinished() { @@ -34,7 +41,7 @@ namespace rip { std::chrono::time_point current = std::chrono::system_clock::now(); units::Time diff = std::chrono::duration_cast(current - m_start_time).count() * units::ms; - misc::Logger::getInstance()->debug("Diff time: {}", diff.to(units::s)); + // misc::Logger::getInstance()->debug("Diff time: {}", diff.to(units::s)); return diff >= m_time; } else @@ -46,8 +53,8 @@ namespace rip void DriveStraight::update(nlohmann::json& state) { - // todo: change velocities based on heading m_pid->calculate(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } void DriveStraight::set(double value) @@ -66,6 +73,7 @@ namespace rip motorcontrollers::MotorDynamics dynamics; dynamics.setSpeed(m_speed); m_drivetrain->drive(dynamics); + m_pid->enable(); } void DriveStraight::teardown(nlohmann::json& state) diff --git a/core/utilities/pid/src/linear_digital_filter.cpp b/core/utilities/pid/src/linear_digital_filter.cpp index f291467..4b8a17c 100644 --- a/core/utilities/pid/src/linear_digital_filter.cpp +++ b/core/utilities/pid/src/linear_digital_filter.cpp @@ -1,7 +1,7 @@ #include "pid/linear_digital_filter.hpp" #include -#include +#include namespace rip { diff --git a/core/utilities/pid/src/pid.cpp b/core/utilities/pid/src/pid.cpp index 0d15978..9f8b57c 100644 --- a/core/utilities/pid/src/pid.cpp +++ b/core/utilities/pid/src/pid.cpp @@ -23,6 +23,11 @@ namespace rip , m_d(d) , m_f(f) , m_setpoint(0) + , m_minimum_input(-1.0) + , m_maximum_input(1.0) + , m_input_range(2.0) + , m_minimum_output(-1.0) + , m_maximum_output(1.0) , m_previous_error(0) , m_total_error(0) , m_last_time(std::chrono::high_resolution_clock::now()) @@ -166,6 +171,9 @@ namespace rip { if (m_original_input == nullptr || m_output == nullptr) { + + misc::Logger::getInstance()->debug_if(m_original_input == nullptr, "Input null"); + misc::Logger::getInstance()->debug_if(m_output == nullptr, "Output null"); return; } @@ -182,9 +190,12 @@ namespace rip // Storage for function inputs m_f = calculateFeedForward(); + misc::Logger::getInstance()->debug("F: {}", m_f); + + // Storage for function input-outputs double error = continuousError(m_setpoint - input); - + misc::Logger::getInstance()->debug("Error: {}", error); if (type == PidInput::Type::kRate) { if (m_p != 0) @@ -249,7 +260,7 @@ namespace rip } } } - return m_error; + return error; } void PidController::reset() From 5df6f9777530f5a958b8d560a1e5c37253dec170 Mon Sep 17 00:00:00 2001 From: solsane Date: Wed, 7 Mar 2018 16:55:03 -0500 Subject: [PATCH 036/200] simple json state storage + path correction --- .../include/navigation_actions/drive_arc.hpp | 37 ++-- core/navigation/actions/src/drive_arc.cpp | 158 +++++++++++++----- 2 files changed, 133 insertions(+), 62 deletions(-) diff --git a/core/navigation/actions/include/navigation_actions/drive_arc.hpp b/core/navigation/actions/include/navigation_actions/drive_arc.hpp index b3962ed..b08da32 100644 --- a/core/navigation/actions/include/navigation_actions/drive_arc.hpp +++ b/core/navigation/actions/include/navigation_actions/drive_arc.hpp @@ -8,6 +8,8 @@ #include #include #include "exceptions.hpp" +#include + namespace rip { @@ -15,11 +17,11 @@ namespace rip { namespace actions { + using Motor = rip::navigation::drivetrains::Drivetrain::Motor; using NavX = navx::NavX; class DriveArc : public framework::Action { public: - using Motor = rip::navigation::drivetrains::Drivetrain::Motor; /** * Drives the robot in an arc with specified radius, angular distance, @@ -32,10 +34,11 @@ namespace rip * @param radius radius size of arc turn * @param axle_length width between left and right wheels. */ - DriveArc(const std::string& name, bool direction, + DriveArc(const std::string& name, bool right, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - const units::Distance& radius, const units::Distance& axle_length); + const units::Distance& radius, const units::Distance& axle_length, + std::shared_ptr navx); /** * Drives the robot in an arc with specified radius, arc length, @@ -48,27 +51,11 @@ namespace rip * @param radius radius size of arc turn * @param axle_length width between left and right wheels. */ - DriveArc(const std::string& name, bool direction, - std::shared_ptr drivetrain, - const units::Velocity& speed, const units::Distance& arc_length, - const units::Distance& radius, const units::Distance& axle_length); - /** - * drivearc. but with a navx. - * @param name action name - * @param direction direction to turn - * @param drivetrain drivetrain - * @param speed linear speed (units) - * @param arc_length linear distance to drive - * @param radius radius to turn around - * @param axle_length length of axle base - * @param navx ptr to navx - */ - DriveArc(const std::string& name, bool direction, + DriveArc(const std::string& name, bool right, std::shared_ptr drivetrain, const units::Velocity& speed, const units::Distance& arc_length, const units::Distance& radius, const units::Distance& axle_length, std::shared_ptr navx); - /** * Returns whether or not the action has finished execution. */ @@ -93,17 +80,23 @@ namespace rip virtual void teardown(nlohmann::json& state) override; private: - bool m_direction; + bool m_right; std::shared_ptr m_drivetrain; units::AngularVelocity m_angular_speed; units::Velocity m_linear_speed; units::Angle m_angle; std::shared_ptr m_navx; + const double k_dead_zone = 0.0001; //percent /** * radius is distance from center of robot/differential drivetrain * to center of circle of arc (ICC) */ - units::Distance m_arc_length, m_radius, m_axle_length, m_traveled, m_init; + units::Distance m_arc_length, m_radius, m_axle_length, m_traveled, m_init_dist; + units::Angle m_init_angle; + std::chrono::time_point m_start_time; + units::Velocity m_velocity_inner, m_velocity_outer; + std::array m_adjusted_speeds; + }; } } diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index b6b8c60..6e15497 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -6,45 +6,31 @@ namespace rip { namespace actions { - DriveArc::DriveArc(const std::string& name, bool direction, + DriveArc::DriveArc(const std::string& name, bool right, std::shared_ptr drivetrain, const units::AngularVelocity& speed, const units::Angle& angle, - const units::Distance& radius, const units::Distance& axle_length) + const units::Distance& radius, const units::Distance& axle_length, + std::shared_ptr navx) : Action(name) - , m_direction(direction) + , m_right(right) , m_drivetrain(drivetrain) , m_angular_speed(speed) , m_angle(angle) , m_radius(radius) , m_axle_length(axle_length) + , m_navx(navx) { m_arc_length = angle.to(units::rad) * radius; m_linear_speed = radius * speed / units::rad; } - DriveArc::DriveArc(const std::string& name, bool direction, - std::shared_ptr drivetrain, - const units::Velocity& speed, const units::Distance& arc_length, - const units::Distance& radius, const units::Distance& axle_length) - : Action(name) - , m_direction(direction) - , m_drivetrain(drivetrain) - , m_linear_speed(speed) - , m_arc_length(arc_length) - , m_radius(radius) - , m_axle_length(axle_length) - { - m_angle = arc_length / radius * units::rad; - m_angular_speed = speed / radius * units::rad; - } - - DriveArc::DriveArc(const std::string& name, bool direction, + DriveArc::DriveArc(const std::string& name, bool right, std::shared_ptr drivetrain, const units::Velocity& speed, const units::Distance& arc_length, const units::Distance& radius, const units::Distance& axle_length, std::shared_ptr navx) : Action(name) - , m_direction(direction) + , m_right(right) , m_drivetrain(drivetrain) , m_linear_speed(speed) , m_arc_length(arc_length) @@ -59,20 +45,105 @@ namespace rip bool DriveArc::isFinished() { m_traveled = readAverageDistance(); - - return m_arc_length <= (m_traveled - m_init); + return m_arc_length <= (m_traveled - m_init_dist); } void DriveArc::update(nlohmann::json& state) { - // todo - return; + std::chrono::time_point current = std::chrono::system_clock::now(); + units::Time diff = std::chrono::duration_cast(current - m_start_time).count() * units::s; + std::array motors; + std::array dynamics; + double diff_state, actual_state, expected_state; + + /** + * Units to be used on the json (since units cannot be serialized) + * Distance: cm + * Time: seconds + * Angle: degrees + */ + // get actual state and expected state in pairs + state[this->name()] = { + {"encoderDistance", { + {"expected", (m_linear_speed * diff).to(units::cm)}, + {"actual", readAverageDistance()} + }}, + {"angle" , { + {"expected", (m_angular_speed * diff).to(units::deg)}, + {"actual", (m_navx->getAngle() - m_init_angle).to(units::deg)} + }}, + {"angularVelocity", { + {"expected", m_angular_speed.to(units::deg / units::s)}, + {"actual", m_navx->getRate().to(units::deg / units::s)} + }}, + {"linearVelocity", { + {"inner", { + {"expected", m_velocity_inner.to(units::cm / units::s)}, + {"actual", m_drivetrain->readEncoderVelocity(motors[0]).to(units::cm / units::s)} + }}, + {"outer", { + {"expected", m_velocity_outer.to(units::cm / units::s)}, + {"actual", m_drivetrain->readEncoderVelocity(motors[1]).to(units::cm / units::s)} + }}, + {"average", { + {"expected", m_linear_speed.to(units::cm / units::s)}, + {"actual", (m_drivetrain->readEncoderVelocity(motors[0]) + + m_drivetrain->readEncoderVelocity(motors[1])).to(units::cm / units::s)/2 } + }} + }}}; + + // adjustments + actual_state = state[this->name()]["encoderDistance"]["actual"]; + expected_state = state[this->name()]["encoderDistance"]["expected"]; + diff_state = actual_state - expected_state; + + // Avg Speed adjustment + if(diff_state > k_dead_zone * expected_state) //actual value larger than expected + { + for(int i=0; i<2; i++) + { + m_adjusted_speeds[i] += 1; + dynamics[i].setSpeed(m_adjusted_speeds[i]); + } + m_drivetrain->drive(dynamics[0], dynamics[1]); + } + else if(diff_state < k_dead_zone * expected_state * -1)//smaller or equal than expected + { + for(int i=0; i<2; i++) + { + m_adjusted_speeds[i] -= 1; + dynamics[i].setSpeed(m_adjusted_speeds[i]); + } + m_drivetrain->drive(dynamics[0], dynamics[1]); + } + + // yaw adjustment + actual_state = state[this->name()]["angle"]["actual"]; + expected_state = state[this->name()]["angle"]["expected"]; + diff_state = actual_state - expected_state; + + if(diff_state > k_dead_zone * expected_state) //angle greater than expected + { + m_adjusted_speeds[0] += 1 * units::cm / units::s; + m_adjusted_speeds[1] -= 1 * units::cm / units::s; + dynamics[0].setSpeed(m_adjusted_speeds[0]); + dynamics[1].setSpeed(m_adjusted_speeds[1]); + m_drivetrain->drive(dynamics[0], dynamics[1]); + } + else if(diff_state < k_dead_zone * expected_state * -1)//smaller or equal than expected + { + m_adjusted_speeds[1] += 1 * units::cm / units::s; + m_adjusted_speeds[0] -= 1 * units::cm / units::s; + dynamics[0].setSpeed(m_adjusted_speeds[0]); + dynamics[1].setSpeed(m_adjusted_speeds[1]); + m_drivetrain->drive(dynamics[0], dynamics[1]); + } } void DriveArc::setup(nlohmann::json& state) { std::vector dist; - units::Velocity v1, v2; + motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; if(m_arc_length <= 0) { @@ -87,13 +158,18 @@ namespace rip throw OutofBoundsException("axle length should be positive"); } - m_init = readAverageDistance(); + m_start_time = std::chrono::system_clock::now(); + m_init_dist = readAverageDistance(); + + do + { + m_init_angle = m_navx->getAngle(); + } while(m_init_angle.to(units::deg) == 0); misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear velocity(in/s): {}" , m_linear_speed.to(units::in / units::s))); - misc::Logger::getInstance()->debug(fmt::format("initial(offset) distance(ft): {}", m_init.to(units::ft))); - motorcontrollers::MotorDynamics dynamicsLeft, dynamicsRight; + misc::Logger::getInstance()->debug(fmt::format("initial(offset) distance(ft): {}", m_init_dist.to(units::ft))); misc::Logger::getInstance()->debug(fmt::format("arc turn intended angular velocity (rev/min): {}" , m_angular_speed.to(units::rev / units::minute))); @@ -104,33 +180,35 @@ namespace rip misc::Logger::getInstance()->debug(fmt::format("arc turn intended linear distance (in): {}" , m_arc_length.to(units::in))); - v1 = m_angular_speed * (m_radius + m_axle_length/2 ) / units::rad; - v2 = m_angular_speed * (m_radius - m_axle_length/2 ) / units::rad; + m_velocity_outer = m_angular_speed * (m_radius + m_axle_length/2 ) / units::rad; + m_velocity_inner = m_angular_speed * (m_radius - m_axle_length/2 ) / units::rad; + m_adjusted_speeds[0] = m_velocity_inner; + m_adjusted_speeds[1] = m_velocity_outer; misc::Logger::getInstance()->debug(fmt::format("arc turn outer motor linear velocity (in/s): {}" - , v1.to(units::in / units::s))); + , m_velocity_outer.to(units::in / units::s))); misc::Logger::getInstance()->debug(fmt::format("arc turn outer motor linear velocity (in/s): {}" - , v2.to(units::in / units::s))); + , m_velocity_inner.to(units::in / units::s))); - if(!m_direction) // left turn + if(!m_right) // left turn { - dynamicsLeft.setSpeed(v2); - dynamicsRight.setSpeed(v1); + dynamicsLeft.setSpeed(m_velocity_inner); + dynamicsRight.setSpeed(m_velocity_outer); } else //right turn { - dynamicsLeft.setSpeed(v1); - dynamicsRight.setSpeed(v2); + dynamicsLeft.setSpeed(m_velocity_outer); + dynamicsRight.setSpeed(m_velocity_inner); } m_drivetrain->drive(dynamicsLeft, dynamicsRight); } units::Distance DriveArc::readAverageDistance() { - std::vector motors = {Motor::kFrontLeft, Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}; units::Distance sum=0; - std::vector dist = m_drivetrain->readEncoders(motors); + std::vector dist = m_drivetrain->readEncoders({Motor::kFrontLeft, + Motor::kFrontLeft, Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}); for(int i=0; i(dist.size()); i++) { sum += dist[i]; From 895309bafafe2f2f3a9d32be788fd411079c485b Mon Sep 17 00:00:00 2001 From: solsane Date: Thu, 8 Mar 2018 12:15:05 -0500 Subject: [PATCH 037/200] dealt with zombie code --- core/navigation/actions/src/drive_arc.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/core/navigation/actions/src/drive_arc.cpp b/core/navigation/actions/src/drive_arc.cpp index 6e15497..eb3a07c 100644 --- a/core/navigation/actions/src/drive_arc.cpp +++ b/core/navigation/actions/src/drive_arc.cpp @@ -62,6 +62,16 @@ namespace rip * Time: seconds * Angle: degrees */ + if(!m_right) + { + motors[0] = Motor::kFrontLeft; + motors[1] = Motor::kFrontRight; + } + else + { + motors[0] = Motor::kFrontRight; + motors[1] = Motor::kFrontLeft; + } // get actual state and expected state in pairs state[this->name()] = { {"encoderDistance", { @@ -208,7 +218,7 @@ namespace rip { units::Distance sum=0; std::vector dist = m_drivetrain->readEncoders({Motor::kFrontLeft, - Motor::kFrontLeft, Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}); + Motor::kFrontRight, Motor::kBackLeft, Motor::kBackRight}); for(int i=0; i(dist.size()); i++) { sum += dist[i]; From aa3c83dffaafb00ce1478b5f56f3dc0835911c15 Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Fri, 9 Mar 2018 15:56:33 -0500 Subject: [PATCH 038/200] get appendages ready for dominate --- appendages/json/servo.json | 3 +++ appendages/json/ultrasonic.json | 4 ++++ appendages/xml/servo.xml | 29 +++++++++++++++++++++++++++++ appendages/xml/ultrasonic.xml | 26 ++++++++++++++++++++++++++ arduino_gen/src/main.cpp | 5 +++-- 5 files changed, 65 insertions(+), 2 deletions(-) create mode 100644 appendages/json/servo.json create mode 100644 appendages/json/ultrasonic.json create mode 100644 appendages/xml/servo.xml create mode 100644 appendages/xml/ultrasonic.xml diff --git a/appendages/json/servo.json b/appendages/json/servo.json new file mode 100644 index 0000000..8bd4e95 --- /dev/null +++ b/appendages/json/servo.json @@ -0,0 +1,3 @@ +{ + "pin": "int" +} diff --git a/appendages/json/ultrasonic.json b/appendages/json/ultrasonic.json new file mode 100644 index 0000000..1367142 --- /dev/null +++ b/appendages/json/ultrasonic.json @@ -0,0 +1,4 @@ +{ + "triggerPin": "int", + "echoPin": "int" +} diff --git a/appendages/xml/servo.xml b/appendages/xml/servo.xml new file mode 100644 index 0000000..a6f78d2 --- /dev/null +++ b/appendages/xml/servo.xml @@ -0,0 +1,29 @@ + + + Servo.h + + + + + + + + + + servos[$i$].attach($pin$); + + + // Servo pin: $pin$ + + + + + + if (!servos[indexNum].attached()) { + servos[indexNum].attach(servo_pins[indexNum]); + } + servos[indexNum].write(value); + + + + diff --git a/appendages/xml/ultrasonic.xml b/appendages/xml/ultrasonic.xml new file mode 100644 index 0000000..52d927b --- /dev/null +++ b/appendages/xml/ultrasonic.xml @@ -0,0 +1,26 @@ + + + NewPing.h + + + + + + + + + + // Ultrasonic triggerPin: $triggerPin$ + + + // Ultrasonic echoPin: $echoPin$ + + + + + + rv = sonar[indexNum].ping_cm(); + + + + diff --git a/arduino_gen/src/main.cpp b/arduino_gen/src/main.cpp index 69920f6..923ccf1 100644 --- a/arduino_gen/src/main.cpp +++ b/arduino_gen/src/main.cpp @@ -6,6 +6,7 @@ #include #include +#include "misc/exception_base.hpp" #include "arduino_gen/arduino_gen.hpp" int main(int argc, char* argv[]) @@ -96,7 +97,7 @@ int main(int argc, char* argv[]) { ag.readConfig(args::get(config)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to read the config file.\n" << e.what() << std::endl; return EXIT_FAILURE; @@ -106,7 +107,7 @@ int main(int argc, char* argv[]) { ag.generateOutput(!args::get(noCopy)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to generate the output.\n" << e.what() << std::endl; return EXIT_FAILURE; From 9e58ae120482e20496a397cb18ffed54127b0314 Mon Sep 17 00:00:00 2001 From: solsane Date: Tue, 13 Mar 2018 19:21:32 -0400 Subject: [PATCH 039/200] roboclaw reset added --- .../motor_controllers/roboclaw/roboclaw.hpp | 4 +- core/motor_controllers/src/roboclaw.cpp | 75 +++++++++++-------- 2 files changed, 47 insertions(+), 32 deletions(-) diff --git a/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp index 5eff2bc..7527e9e 100644 --- a/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp +++ b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp @@ -26,7 +26,7 @@ #include #include #include - +#include #include #include @@ -793,7 +793,7 @@ namespace rip //advanced unsigned int m_databits, m_stopbits; bool m_xonxoff, m_rtscts; - bool m_faking = false; + bool m_faking = false, m_advanced_serial = false; serial_parity_t m_parity; //TODO: support advanced serial opening diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index 7abbe4d..b77f769 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -61,10 +61,12 @@ namespace rip m_parity = config.at("parity"); } } - if (config.find("faking") == config.end()) + m_faking = !(config.find("faking") == config.end()); + if(m_faking) { if (config.find("advanced serial options") != config.end()) { + m_advanced_serial = true; open(&m_serial, m_device, m_baudrate, m_databits, m_parity, m_stopbits, m_xonxoff, m_rtscts); } @@ -73,10 +75,6 @@ namespace rip open(&m_serial, m_device, m_baudrate); } } - else - { - m_faking = true; - } } Roboclaw::~Roboclaw() { @@ -694,51 +692,68 @@ namespace rip uint8_t value = 0; uint8_t trys = kMaxRetries; int16_t data; + uint8_t max_reset = 2; std::vector command = {m_address, static_cast(cmd)}; - for (uint8_t try_ = 0; try_ < kMaxRetries; try_++) + for(uint8_t reset = 0; reset < max_reset; reset++) { - crcClear(); - for (uint8_t i = 0; i < command.size(); i++) + for (uint8_t try_ = 0; try_ < kMaxRetries; try_++) { - crcUpdate(command[i]); - } + crcClear(); + for (uint8_t i = 0; i < command.size(); i++) + { + crcUpdate(command[i]); + } - write(&m_serial, command, command.size()); + write(&m_serial, command, command.size()); - std::vector response; - uint8_t data; - for (uint8_t i = 0; i < n; i++) - { - data = read(&m_serial, m_timeout); - crcUpdate(data); - response.push_back(data); - if (data == -1) + std::vector response; + uint8_t data; + for (uint8_t i = 0; i < n; i++) { - continue; + data = read(&m_serial, m_timeout); + crcUpdate(data); + response.push_back(data); + if (data == -1) + { + continue; + } } - } - if (data != -1) - { - uint16_t ccrc; - data = read(&m_serial, m_timeout); if (data != -1) { - ccrc = static_cast(data) << 8; + uint16_t ccrc; data = read(&m_serial, m_timeout); if (data != -1) { - ccrc |= data; - if (crcGet() == ccrc) + ccrc = static_cast(data) << 8; + data = read(&m_serial, m_timeout); + if (data != -1) { - return response; + ccrc |= data; + if (crcGet() == ccrc) + { + return response; + } } } } } + misc::Logger::getInstance()->debug(fmt::format("Serial read failure on device {}, attempting reset...", m_device)); + if(!m_faking) + { + serial_close(&m_serial); + if(m_advanced_serial) + { + open(&m_serial, m_device, m_baudrate, m_databits, m_parity, + m_stopbits, m_xonxoff, m_rtscts); + } + else + { + open(&m_serial, m_device, m_baudrate); + } + } } - throw ReadFailure(); } From 6f295aa98348958dad158471e75446e86d464d47 Mon Sep 17 00:00:00 2001 From: solsane Date: Tue, 13 Mar 2018 20:47:50 -0400 Subject: [PATCH 040/200] fix to parity --- core/motor_controllers/src/roboclaw.cpp | 78 ++++++++----------------- 1 file changed, 25 insertions(+), 53 deletions(-) diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index b77f769..1106586 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -49,39 +49,55 @@ namespace rip m_ticks_per_rev = config.at("ticks_per_rev"); m_wheel_radius = config.at("wheel_radius"); //serial - std::string temp = config.at("device"); - m_device = temp.c_str(); + m_device = config.at("device"); m_baudrate = config.at("baudrate"); if (config.find("advanced serial options") != config.end()) { + serial_parity cpar; + int par = config.at("parity"); + if (parity == 0) + { + cpar = PARITY_NONE; + } + else if (parity == 1) + { + cpar = PARITY_ODD; + } + else + { + cpar = PARITY_EVEN; + } m_databits = config.at("databits"); m_stopbits = config.at("stopbits"); m_xonxoff = config.at("xonxoff"); m_rtscts = config.at("rtscts"); - m_parity = config.at("parity"); + m_parity = cpar; } } - m_faking = !(config.find("faking") == config.end()); - if(m_faking) + if (config.find("faking") == config.end()) { if (config.find("advanced serial options") != config.end()) { - m_advanced_serial = true; - open(&m_serial, m_device, m_baudrate, m_databits, m_parity, + m_serial.open(m_device, m_baudrate, m_databits, m_parity, m_stopbits, m_xonxoff, m_rtscts); } else { - open(&m_serial, m_device, m_baudrate); + m_serial.open(m_device, m_baudrate); } } + else + { + m_faking = true; + } } + Roboclaw::~Roboclaw() { if(!m_faking) { stop(); - serial_close(&m_serial); + m_serial.close(); } } @@ -93,8 +109,6 @@ namespace rip { Command cmd; switch (motor) - { - case Motor::kM1: cmd = Command::kM1Duty; break; case Motor::kM2: @@ -1029,45 +1043,3 @@ namespace rip throw BadJson("Not enough config values, min size: " + std::to_string(7)); } - for (int i = 1; i < 5; i++) - { - if (testcfg[vars[i]] < 0.0) - { - throw OutOfRange(vars[i] + " should be a positive value"); - } - } - if (testcfg["baudrate"] < 0) - { - throw OutOfRange("baudrate should be positive."); - } - if (testcfg["wheel_radius"] == 0) - { - throw OutOfRange("wheel radius cannot = 0"); - } - try - { - setName(testcfg["name"]); - m_address = testcfg.at("address").get(); - m_timeout = testcfg.at("timeout"); - m_ticks_per_rev = testcfg.at("ticks_per_rev"); - m_wheel_radius = testcfg.at("wheel_radius"); - std::string temp = testcfg.at("device"); - m_device = temp.c_str(); - m_baudrate = testcfg.at("baudrate"); - if (testcfg.find("advanced serial options") != testcfg.end()) - { - m_databits = testcfg.at("databits"); - m_stopbits = testcfg.at("stopbits"); - m_xonxoff = testcfg.at("xonxoff"); - m_rtscts = testcfg.at("rtscts"); - m_parity = testcfg.at("parity"); - } - } - catch (...) - { - throw BadJson("Failed to set 1 or more values"); - } - } - } - } -} From c467e585282f8e11f0f1866f22e3acdad1e02d32 Mon Sep 17 00:00:00 2001 From: Aiden Date: Wed, 14 Mar 2018 11:30:14 -0400 Subject: [PATCH 041/200] broken commit fix --- core/motor_controllers/src/roboclaw.cpp | 52 ++++++++++++++++++------- 1 file changed, 38 insertions(+), 14 deletions(-) diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index b77f769..398d662 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -58,7 +58,21 @@ namespace rip m_stopbits = config.at("stopbits"); m_xonxoff = config.at("xonxoff"); m_rtscts = config.at("rtscts"); - m_parity = config.at("parity"); + int parity = config.at("parity"); + serial_parity_t cpar; + if (parity == 0) + { + cpar = PARITY_NONE; + } + else if (parity == 1) + { + cpar = PARITY_ODD; + } + else + { + cpar = PARITY_EVEN; + } + m_parity = cpar; } } m_faking = !(config.find("faking") == config.end()); @@ -360,7 +374,7 @@ namespace rip for (uint8_t i = 0; i < 48; i++) { - if (data != -1) + if (data != 0xFF) { data = read(&m_serial, m_timeout); version += data; @@ -369,12 +383,12 @@ namespace rip { uint16_t ccrc; data = read(&m_serial, m_timeout); - if (data != -1) + if (data != 0xFF) { ccrc = static_cast(data) << 8; data = read(&m_serial, m_timeout); - if (data != -1) + if (data != 0xFF) { ccrc |= data; if (crcGet() == ccrc) @@ -687,11 +701,6 @@ namespace rip ////////////////////////////////////////////////////////////////////////////////////////////// std::vector Roboclaw::readN(uint8_t n, Command cmd) { - uint8_t crc; - - uint8_t value = 0; - uint8_t trys = kMaxRetries; - int16_t data; uint8_t max_reset = 2; std::vector command = {m_address, static_cast(cmd)}; @@ -714,21 +723,21 @@ namespace rip data = read(&m_serial, m_timeout); crcUpdate(data); response.push_back(data); - if (data == -1) + if (data == 0xFF) { continue; } } - if (data != -1) + if (data != 0xFF) { uint16_t ccrc; data = read(&m_serial, m_timeout); - if (data != -1) + if (data != 0xFF) { ccrc = static_cast(data) << 8; data = read(&m_serial, m_timeout); - if (data != -1) + if (data != 0xFF) { ccrc |= data; if (crcGet() == ccrc) @@ -960,6 +969,7 @@ namespace rip bool Roboclaw::diagnostic() { // todo + return 0; } void Roboclaw::crcUpdate(uint8_t data) @@ -1060,7 +1070,21 @@ namespace rip m_stopbits = testcfg.at("stopbits"); m_xonxoff = testcfg.at("xonxoff"); m_rtscts = testcfg.at("rtscts"); - m_parity = testcfg.at("parity"); + int parity = testcfg.at("parity"); + serial_parity_t cpar; + if (parity == 0) + { + cpar = PARITY_NONE; + } + else if (parity == 1) + { + cpar = PARITY_ODD; + } + else + { + cpar = PARITY_EVEN; + } + m_parity = cpar; } } catch (...) From 56febf510288db22cb6bb5d4553ed7e802b1f98e Mon Sep 17 00:00:00 2001 From: Aiden Date: Wed, 14 Mar 2018 11:52:35 -0400 Subject: [PATCH 042/200] testing fix --- core/motor_controllers/src/roboclaw.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index 398d662..9ba71cf 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -76,7 +76,7 @@ namespace rip } } m_faking = !(config.find("faking") == config.end()); - if(m_faking) + if(!m_faking) { if (config.find("advanced serial options") != config.end()) { From b6975e077290ee01da99eb25808d881775bc841c Mon Sep 17 00:00:00 2001 From: solsane Date: Sun, 18 Mar 2018 13:21:40 -0400 Subject: [PATCH 043/200] Minor fixes --- .../diag/diag_roboclaw_main.cpp | 17 ++++++++++---- core/motor_controllers/src/roboclaw.cpp | 23 ++++++++++++++----- .../src/two_roboclaw_drivetrain.cpp | 1 - 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/core/motor_controllers/diag/diag_roboclaw_main.cpp b/core/motor_controllers/diag/diag_roboclaw_main.cpp index a3976ec..9468f31 100644 --- a/core/motor_controllers/diag/diag_roboclaw_main.cpp +++ b/core/motor_controllers/diag/diag_roboclaw_main.cpp @@ -1,4 +1,4 @@ -#pragma GCC diagnostic push +#pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wunused-variable" #pragma GCC diagnostic ignored "-Wunused-parameter" #pragma GCC diagnostic ignored "-Wswitch-default" @@ -24,22 +24,24 @@ int main(int argc, char** argv) //make serial stuff nlohmann::json config1 = { + {"name", "clawRight"}, {"address", 0x80}, {"timeout", 1000}, {"ticks_per_rev", 3591.84}, {"wheel_radius", (2.875 / 2.0)* rip::units::in}, {"device", "/dev/ttyAMA0"}, - {"baudrate", 115200} + {"baudrate", 38400} }; nlohmann::json config2 = config1; config2["address"] = 0x81; + config2["name"] = "clawLeft"; Roboclaw testClawLeft(config2); Roboclaw testClawRight(config1); std::vector claws = {&testClawLeft, &testClawRight}; - diag::Diag diagTool(claws); - diagTool.start(); + //diag::Diag diagTool(claws); + //diagTool.start(); /* MotorDynamics dynamics, stop; stop.setSpeed(0); @@ -60,7 +62,12 @@ int main(int argc, char** argv) testClawLeft.setDynamics(dynamics); testClawRight.setDynamics(stop, false); testClawLeft.setDynamics(stop, false); - + */ + while(1) + { + std::cout << testClawLeft.readEncodersRaw()[0] << std::endl; + //std::cout << testClawRight.readEncodersRaw()[0] << std::endl; + } //std::cout << "ticks m1 left " << testClawLeft.readEncoderRaw(Roboclaw::Motor::kM1) << std::endl; /* testClawRight.resetEncoders(); diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index 398d662..cb39286 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -45,7 +45,8 @@ namespace rip setName(config["name"]); //required parameters m_address = config.at("address").get(); - m_timeout = config.at("timeout"); + m_timeout = config.at("timeout") * units::ms; + m_ticks_per_rev = config.at("ticks_per_rev"); m_wheel_radius = config.at("wheel_radius"); //serial @@ -75,8 +76,8 @@ namespace rip m_parity = cpar; } } - m_faking = !(config.find("faking") == config.end()); - if(m_faking) + m_faking = (config.find("faking") != config.end()); + if(!m_faking) { if (config.find("advanced serial options") != config.end()) { @@ -748,9 +749,10 @@ namespace rip } } } - misc::Logger::getInstance()->debug(fmt::format("Serial read failure on device {}, attempting reset...", m_device)); if(!m_faking) { + misc::Logger::getInstance()->debug(fmt::format("Serial read failure on device {}, attempting reset...", m_device)); + serial_flush(&m_serial); serial_close(&m_serial); if(m_advanced_serial) { @@ -770,6 +772,8 @@ namespace rip { if (serial_write(serial, &command[0], len) < 0) { + misc::Logger::getInstance()->debug(fmt::format("Serial write failure on device {}", m_device)); + throw CommandFailure(serial_errmsg(serial)); } } @@ -777,8 +781,10 @@ namespace rip uint8_t Roboclaw::read(serial_t* serial, units::Time timeout) { uint8_t data; - if (serial_read(serial, &data, 1, static_cast(timeout())) < 0) + if (serial_read(serial, &data, 1, static_cast(timeout.to(units::ms))) < 0) { + misc::Logger::getInstance()->debug(fmt::format("Serial read failure on device {}", m_device)); + throw ReadFailure(serial_errmsg(serial)); } return data; @@ -792,6 +798,8 @@ namespace rip if (serial_open_advanced(serial, device.c_str(), baudrate, databits, parity, stopbits, xonxoff, rtscts) < 0) { + misc::Logger::getInstance()->debug(fmt::format("Serial open failure on device {}", m_device)); + throw SerialOpenFail(serial_errmsg(serial)); } } @@ -800,6 +808,8 @@ namespace rip { if (serial_open(serial, device.c_str(), baudrate) < 0) { + misc::Logger::getInstance()->debug(fmt::format("Serial open failure on device {}", m_device)); + throw SerialOpenFail(serial_errmsg(serial)); } } @@ -1058,7 +1068,8 @@ namespace rip { setName(testcfg["name"]); m_address = testcfg.at("address").get(); - m_timeout = testcfg.at("timeout"); + m_timeout = testcfg.at("timeout") * units::ms; + misc::Logger::getInstance()->debug(fmt::format("Roboclaw timeout in ms = {}", m_timeout.to(units::ms))); m_ticks_per_rev = testcfg.at("ticks_per_rev"); m_wheel_radius = testcfg.at("wheel_radius"); std::string temp = testcfg.at("device"); diff --git a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp index fdf0c82..f92ab42 100644 --- a/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp +++ b/core/navigation/drivetrains/src/two_roboclaw_drivetrain.cpp @@ -90,7 +90,6 @@ namespace rip std::vector data; for(uint i=0; i(motors[i]) << std::endl; switch(motors[i]) { From 9bf607381e6d7c8e2ce77c472ddcc8b7eff5e9e4 Mon Sep 17 00:00:00 2001 From: Parker Mitchell Date: Sun, 18 Mar 2018 14:34:10 -0400 Subject: [PATCH 044/200] testing quick fix for serial --- .../include/motor_controllers/roboclaw/roboclaw.hpp | 2 ++ core/motor_controllers/src/roboclaw.cpp | 4 ++++ 2 files changed, 6 insertions(+) diff --git a/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp index 7527e9e..c9a4d7c 100644 --- a/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp +++ b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -663,6 +664,7 @@ namespace rip virtual void stop() override; virtual bool diagnostic() override; + static std::mutex global_lock; private: static const uint8_t kMaxRetries = 10; diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index 1719bcf..041c373 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -703,6 +703,9 @@ namespace rip ////////////////////////////////////////////////////////////////////////////////////////////// std::vector Roboclaw::readN(uint8_t n, Command cmd) { + // todo: don't do this --- just trying something out + std::lock_guard lock(Roboclaw::global_lock); + uint8_t max_reset = 2; std::vector command = {m_address, static_cast(cmd)}; @@ -777,6 +780,7 @@ namespace rip throw CommandFailure(serial_errmsg(serial)); } + serial_flush(serial); } uint8_t Roboclaw::read(serial_t* serial, units::Time timeout) From 639373f32d8ed6f1e6242e7d2a570b353ba8cf8b Mon Sep 17 00:00:00 2001 From: Parker Mitchell Date: Sun, 18 Mar 2018 16:06:27 -0400 Subject: [PATCH 045/200] more messing around with roboclaw --- .../include/motor_controllers/roboclaw/roboclaw.hpp | 6 ++++-- core/motor_controllers/src/roboclaw.cpp | 9 +++++---- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp index c9a4d7c..8b6f162 100644 --- a/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp +++ b/core/motor_controllers/include/motor_controllers/roboclaw/roboclaw.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -157,6 +158,7 @@ namespace rip kFlagBootLoader = 255 }; // enum class Command + static std::mutex global_lock; /** * @brief Constructor @@ -664,7 +666,7 @@ namespace rip virtual void stop() override; virtual bool diagnostic() override; - static std::mutex global_lock; + private: static const uint8_t kMaxRetries = 10; @@ -789,7 +791,7 @@ namespace rip double m_ticks_per_rev; units::Distance m_wheel_radius; //serial members - const char* m_device; + std::string m_device; uint32_t m_baudrate; serial_t m_serial; //advanced diff --git a/core/motor_controllers/src/roboclaw.cpp b/core/motor_controllers/src/roboclaw.cpp index 041c373..251946a 100644 --- a/core/motor_controllers/src/roboclaw.cpp +++ b/core/motor_controllers/src/roboclaw.cpp @@ -29,6 +29,9 @@ namespace rip { namespace roboclaw { + + std::mutex Roboclaw::global_lock; + Roboclaw::Roboclaw(const nlohmann::json& config, bool test) : Subsystem("") { @@ -50,8 +53,7 @@ namespace rip m_ticks_per_rev = config.at("ticks_per_rev"); m_wheel_radius = config.at("wheel_radius"); //serial - std::string temp = config.at("device"); - m_device = temp.c_str(); + m_device = config.at("device"); m_baudrate = config.at("baudrate"); if (config.find("advanced serial options") != config.end()) { @@ -1077,8 +1079,7 @@ namespace rip misc::Logger::getInstance()->debug(fmt::format("Roboclaw timeout in ms = {}", m_timeout.to(units::ms))); m_ticks_per_rev = testcfg.at("ticks_per_rev"); m_wheel_radius = testcfg.at("wheel_radius"); - std::string temp = testcfg.at("device"); - m_device = temp.c_str(); + m_device = testcfg.at("device"); m_baudrate = testcfg.at("baudrate"); if (testcfg.find("advanced serial options") != testcfg.end()) { From 3fad0ddeda28dd23dd68a5e10651dfb963ce801e Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Mon, 19 Mar 2018 05:10:35 -0500 Subject: [PATCH 046/200] Added roboclaw xml and json Added string_literal type and prefix/suffix attributes to argument. --- appendages/json/roboclaw.json | 4 + appendages/xml/roboclaw.xml | 90 ++++++++++++++++++++ arduino_gen/include/arduino_gen/argument.hpp | 2 + arduino_gen/src/argument.cpp | 36 ++++++-- arduino_gen/src/constructor.cpp | 8 +- arduino_gen/src/main.cpp | 6 +- 6 files changed, 132 insertions(+), 14 deletions(-) create mode 100644 appendages/json/roboclaw.json create mode 100644 appendages/xml/roboclaw.xml diff --git a/appendages/json/roboclaw.json b/appendages/json/roboclaw.json new file mode 100644 index 0000000..b7553b1 --- /dev/null +++ b/appendages/json/roboclaw.json @@ -0,0 +1,4 @@ +{ + "serial": "string", + "baudrate": "int" +} \ No newline at end of file diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml new file mode 100644 index 0000000..96c6936 --- /dev/null +++ b/appendages/xml/roboclaw.xml @@ -0,0 +1,90 @@ + + + RoboClaw.h + + + + + + + + + roboclaws[$i$].begin($baudrate$); + + + + + + + + if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kSetSpeed); + } + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); + } + + + + + + + + if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kReadEncoders); + } + + + + + + + + if (!roboclaws[indexNum].DutyM1M2(address, duty1, duty2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kSetDuty); + } + + + + + + + + + + + bool result; + + switch (motor) + { + case 1: + result = roboclaws[indexNum].SetM1VelocityPID(address, Kp, Ki, Kd, qpps); + break; + case 2: + result = roboclaws[indexNum].SetM2VelocityPID(address, Kp, Ki, Kd, qpps); + break; + default: + result = false; + break; + } + + if (!result) // TODO: Double check return values + { + cmdMessenger.sendBinCmd(kError, kSetVelocityPID); + } + + + + diff --git a/arduino_gen/include/arduino_gen/argument.hpp b/arduino_gen/include/arduino_gen/argument.hpp index 30e6481..aaae0fe 100644 --- a/arduino_gen/include/arduino_gen/argument.hpp +++ b/arduino_gen/include/arduino_gen/argument.hpp @@ -56,6 +56,8 @@ namespace rip std::string m_name; std::string m_type; std::string m_value; + std::string m_prefix; + std::string m_suffix; }; } // arduinogen } diff --git a/arduino_gen/src/argument.cpp b/arduino_gen/src/argument.cpp index f8d6640..f975af2 100644 --- a/arduino_gen/src/argument.cpp +++ b/arduino_gen/src/argument.cpp @@ -27,10 +27,30 @@ namespace rip m_value = ""; } + m_prefix = ""; + m_suffix = ""; + if (m_type == "string_literal") + { + try + { + m_prefix = getAttribute("prefix")->Value(); + } + catch (AttributeException) + { } + + try + { + m_suffix = getAttribute("suffix")->Value(); + } + catch (AttributeException) + { } + } + if(m_type != "float" && m_type != "int" && m_type != "bool" && - m_type != "string") + m_type != "string" && + m_type != "string_literal") { throw AttributeException(fmt::format("Constructor argument unknown type on line number {}", xml->GetLineNum())); @@ -53,13 +73,13 @@ namespace rip std::string Argument::toString(std::shared_ptr appendage) const { - if (appendage->has(m_name)) - { - if(!appendage->isType(m_name, m_type)) - { - // TODO(Andrew): throw exception - } - return appendage->getString(m_name); + if (appendage->has(m_name)) + { + if (!appendage->isType(m_name, m_type == "string_literal" ? "string" : m_type)) + { + // TODO(Andrew): throw exception + } + return m_prefix + appendage->getString(m_name) + m_suffix; } else { diff --git a/arduino_gen/src/constructor.cpp b/arduino_gen/src/constructor.cpp index 806ac3b..aaa095e 100644 --- a/arduino_gen/src/constructor.cpp +++ b/arduino_gen/src/constructor.cpp @@ -66,10 +66,10 @@ namespace rip { if (appendage->has(argument.getName())) { - if (appendage->isType(argument.getName(), "string")) - { - rv += fmt::format("\"{}\"", argument.toString(appendage)); - } + if (argument.getType() == "string") + { + rv += fmt::format("\"{}\"", argument.toString(appendage)); + } else { rv += argument.toString(appendage); diff --git a/arduino_gen/src/main.cpp b/arduino_gen/src/main.cpp index 69920f6..a580887 100644 --- a/arduino_gen/src/main.cpp +++ b/arduino_gen/src/main.cpp @@ -8,6 +8,8 @@ #include "arduino_gen/arduino_gen.hpp" +#include + int main(int argc, char* argv[]) { args::ArgumentParser parser("ArduinoGen generates arduino code to be used with RIP."); @@ -96,7 +98,7 @@ int main(int argc, char* argv[]) { ag.readConfig(args::get(config)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to read the config file.\n" << e.what() << std::endl; return EXIT_FAILURE; @@ -106,7 +108,7 @@ int main(int argc, char* argv[]) { ag.generateOutput(!args::get(noCopy)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to generate the output.\n" << e.what() << std::endl; return EXIT_FAILURE; From c8b5b0b2b548b74bb35130343e0c703e144f3042 Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Tue, 20 Mar 2018 17:41:32 -0400 Subject: [PATCH 047/200] beginning of localization --- core/navigation/CMakeLists.txt | 1 + core/navigation/localization/CMakeLists.txt | 27 + .../include/localization/action_model.hpp | 21 + .../extended_information_filter.hpp | 76 +++ .../localization/extended_kalman_filter.hpp | 125 +++++ .../localization/gaussian_action_model.hpp | 31 ++ .../gaussian_sensor_model.hpp.hpp | 27 + .../localization/information_filter.hpp.hpp | 70 +++ .../include/localization/kalman_filter.hpp | 234 ++++++++ .../localization/markov_action_chain.hpp | 182 ++++++ .../probability/bayes_distribution.hpp | 95 ++++ .../probability/conditional_map.hpp | 52 ++ .../continuous/gaussian_canonical.hpp | 134 +++++ .../continuous/gaussian_moment.hpp | 132 +++++ .../localization/probability/data/matrix.hpp | 81 +++ .../data/matrix_vector_operators.hpp | 25 + .../localization/probability/data/vector.hpp | 73 +++ .../probability/data/vector_range.hpp | 64 +++ .../discrete/conditional_value_map.hpp | 170 ++++++ .../discrete/discrete_conditional.hpp | 36 ++ .../discrete/discrete_distribution.hpp | 48 ++ .../discrete/distribution_counter.hpp | 65 +++ .../discrete/distribution_value_map.hpp | 118 ++++ .../probability/discrete/markov_chain.hpp | 158 ++++++ .../independent_pair_distribution.hpp | 54 ++ .../probability/random_conditional.hpp | 29 + .../probability/random_distribution.hpp | 31 ++ .../probability/random_uniform.hpp | 54 ++ .../probability/ranged_uniform.hpp | 51 ++ .../include/localization/sensor_model.hpp | 23 + .../src/probability/data/matrix.cpp | 524 ++++++++++++++++++ .../data/matrix_vector_operators.cpp | 72 +++ .../src/probability/data/vector.cpp | 209 +++++++ core/navigation/localization/test/main.cpp | 8 + .../test/test_bayes_distribution.cpp | 111 ++++ .../test/test_conditional_map.hpp.cpp | 38 ++ .../test/test_conditional_value_map.cpp | 132 +++++ .../test_independent_pair_distribution.cpp | 75 +++ .../test/test_random_distribution.cpp | 46 ++ .../localization/test/test_ranged_uniform.cpp | 44 ++ 40 files changed, 3546 insertions(+) create mode 100644 core/navigation/localization/CMakeLists.txt create mode 100644 core/navigation/localization/include/localization/action_model.hpp create mode 100644 core/navigation/localization/include/localization/extended_information_filter.hpp create mode 100644 core/navigation/localization/include/localization/extended_kalman_filter.hpp create mode 100644 core/navigation/localization/include/localization/gaussian_action_model.hpp create mode 100644 core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp create mode 100644 core/navigation/localization/include/localization/information_filter.hpp.hpp create mode 100644 core/navigation/localization/include/localization/kalman_filter.hpp create mode 100644 core/navigation/localization/include/localization/markov_action_chain.hpp create mode 100644 core/navigation/localization/include/localization/probability/bayes_distribution.hpp create mode 100644 core/navigation/localization/include/localization/probability/conditional_map.hpp create mode 100644 core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp create mode 100644 core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp create mode 100644 core/navigation/localization/include/localization/probability/data/matrix.hpp create mode 100644 core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp create mode 100644 core/navigation/localization/include/localization/probability/data/vector.hpp create mode 100644 core/navigation/localization/include/localization/probability/data/vector_range.hpp create mode 100644 core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp create mode 100644 core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp create mode 100644 core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp create mode 100644 core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp create mode 100644 core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp create mode 100644 core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp create mode 100644 core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp create mode 100644 core/navigation/localization/include/localization/probability/random_conditional.hpp create mode 100644 core/navigation/localization/include/localization/probability/random_distribution.hpp create mode 100644 core/navigation/localization/include/localization/probability/random_uniform.hpp create mode 100644 core/navigation/localization/include/localization/probability/ranged_uniform.hpp create mode 100644 core/navigation/localization/include/localization/sensor_model.hpp create mode 100644 core/navigation/localization/src/probability/data/matrix.cpp create mode 100644 core/navigation/localization/src/probability/data/matrix_vector_operators.cpp create mode 100644 core/navigation/localization/src/probability/data/vector.cpp create mode 100644 core/navigation/localization/test/main.cpp create mode 100644 core/navigation/localization/test/test_bayes_distribution.cpp create mode 100644 core/navigation/localization/test/test_conditional_map.hpp.cpp create mode 100644 core/navigation/localization/test/test_conditional_value_map.cpp create mode 100644 core/navigation/localization/test/test_independent_pair_distribution.cpp create mode 100644 core/navigation/localization/test/test_random_distribution.cpp create mode 100644 core/navigation/localization/test/test_ranged_uniform.cpp diff --git a/core/navigation/CMakeLists.txt b/core/navigation/CMakeLists.txt index d937a43..0807fe9 100644 --- a/core/navigation/CMakeLists.txt +++ b/core/navigation/CMakeLists.txt @@ -6,3 +6,4 @@ add_subdirectory(path_planner) add_subdirectory(actions) add_subdirectory(drivetrains) add_subdirectory(navx) +add_subdirectory(localization) diff --git a/core/navigation/localization/CMakeLists.txt b/core/navigation/localization/CMakeLists.txt new file mode 100644 index 0000000..3c2a281 --- /dev/null +++ b/core/navigation/localization/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.1) +message("Building localization") +# define the project +project(localization) + +# create and configure the library target +file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/**/**.cpp) +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/**/**.hpp) +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) +# target_link_libraries(${PROJECT_NAME} fmt json units spdlog motor_controllers navx misc) +target_include_directories(${PROJECT_NAME} PUBLIC include) + +# if(ENABLE_TESTING) + # Get test source files and test data files + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") + #file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") + + # Copy test data to outdir/test + #make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) + + # Create localization_test executable, set to c++11, link localization library and google test libraries + add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES})# ${testDataOutputs}) + set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_test gmock gtest googletest_rip_macros) +# endif() diff --git a/core/navigation/localization/include/localization/action_model.hpp b/core/navigation/localization/include/localization/action_model.hpp new file mode 100644 index 0000000..fc3d165 --- /dev/null +++ b/core/navigation/localization/include/localization/action_model.hpp @@ -0,0 +1,21 @@ +#ifndef ACTION_MODEL_HPP +#define ACTION_MODEL_HPP + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class ActionModel + { + public: + ActionModel() = default; + virtual ~ActionModel() = default; + }; + } + } +} + +#endif // ACTION_MODEL_HPP diff --git a/core/navigation/localization/include/localization/extended_information_filter.hpp b/core/navigation/localization/include/localization/extended_information_filter.hpp new file mode 100644 index 0000000..e39e2ff --- /dev/null +++ b/core/navigation/localization/include/localization/extended_information_filter.hpp @@ -0,0 +1,76 @@ +#ifndef EXTENDED_INFORMATION_FILTER_HPP +#define EXTENDED_INFORMATION_FILTER_HPP + +#include "extended_kalman_filter.hpp" +#include "probability/continuous/gaussian_canonical.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class ExtendedInformationFilter + { + using probability::Matrix; + using probability::Vector; + using probability::GaussianCanonical; + public: + ExtendedInformationFilter() {} + virtual ~ExtendedInformationFilter() {} + + std::shared_ptr< GaussianCanonical > marginalize( + const ExtendedKalmanActionModel& model, + const U& action, + const GaussianCanonical& belief) + { + + + const X& mu = belief.getMu(); + const Matrix& Sigma = belief.inversePrecision(); + + Matrix G = model.getStateJacobian(action, mu); + Matrix Gt = G.transpose(); + Matrix R = model.getError(action, mu).inverse(); + Matrix nextOmega = (G * Sigma * Gt + R).inverse(); + + X nextXi; + nextXi.set(nextOmega * model.getMean(action, mu)); + + return std::make_shared>(nextXi, nextOmega); + } + + std::shared_ptr> + bayesianInference( + const ExtendedKalmanSensorModel& model, + const Z& observation, + const probability::continuous::GaussianCanonical& belief) + { + using probability::Matrix; + using probability::Vector; + using probability::continuous::GaussianCanonical; + + const X& mu = belief.getMu(); + const Matrix& Omega = belief.precision(); + + Matrix H = model.getJacobian(mu); + Matrix Ht = H.transpose(); + Matrix Q = model.getError(observation); + Matrix QInv = Q.inverse(); + Matrix nextOmega = Omega + Ht * QInv * H; + + const Vector& xi = belief.information(); + Z expectedObservation = model.getMean(mu); + + Vector nextXi = xi + Ht * QInv * (observation - expectedObservation + H * mu); + + return std::make_shared>(nextXi, nextOmega); + } + }; + + } + } +} + +#endif // EXTENDED_INFORMATION_FILTER_HPP diff --git a/core/navigation/localization/include/localization/extended_kalman_filter.hpp b/core/navigation/localization/include/localization/extended_kalman_filter.hpp new file mode 100644 index 0000000..22ae638 --- /dev/null +++ b/core/navigation/localization/include/localization/extended_kalman_filter.hpp @@ -0,0 +1,125 @@ +#ifndef EXTENDED_KALMAN_FILTER_HPP +#define EXTENDED_KALMAN_FILTER_HPP + +#include "probability/data/matrix.hpp" +#include "probability/continuous/gaussian_moment.hpp" + + +namespace rip +{ + namespace navigation + { + namespace localization + { + + using Matrix = probability::Matrix; + + template + class ExtendedKalmanActionModel : public GaussianActionModel + { + public: + ExtendedKalmanActionModel() {} + virtual ~ExtendedKalmanActionModel() {} + + virtual Matrix GetStateJacobian(const U& action, const X& state) const = 0; + virtual Matrix GetActionJacobian(const U& action, const X& state) const = 0; + }; + + template + class ExtendedKalmanSensorModel : public GaussianSensorModel + { + public: + ExtendedKalmanSensorModel() {} + virtual ~ExtendedKalmanSensorModel() {} + + virtual Matrix GetJacobian(const X& x) const = 0; + }; + + template + class ExtendedKalmanFilter + { + using probability::GaussianMoment; + using probability::Vector; + public: + ExtendedKalmanFilter() {} + virtual ~ExtendedKalmanFilter() {} + + std::shared_ptr< GaussianMoment > marginalize( + const ExtendedKalmanActionModel& model, + const U& action, + const GaussianMoment& belief) + { + + X x0 = belief.mean(); + Matrix e0 = belief.covariance(); + + // time update + Matrix g = model.getStateJacobian(action, x0); + Matrix gt = g.transpose(); + Matrix r = model.getError(action, x0); + + X x1 = model.getMean(action, x0); + Matrix e1 = g * e0 * gt + r; + + return std::make_shared>(x1, e1); + } + + std::shared_ptr> marginalize( + const ExtendedKalmanActionModel& model, + const GaussianMoment& action, + const GaussianMoment& belief) + { + + X x0 = belief.mean(); + U u = action.mean(); + Matrix e0 = belief.covariance(); + Matrix eu = action.covariance(); + + // time update + Matrix gx = model.getStateJacobian(u, x0); + Matrix gxt = gx.transpose(); + Matrix gu = model.getActionJacobian(action.Mean, x0); + Matrix gut = gu.transpose(); + Matrix r = model.getError(u, x0); + + X x1 = model.getMean(u, x0); + Matrix e1 = gx * e0 * gxt + gu * eu * gut + r; + + return std::make_shared>(x1, e1); + } + + + std::shared_ptr> bayesianInference( + const ExtendedKalmanSensorModel& sensor_model, + const Z& observation, + const GaussianMoment& belief) + { + + // Table 3.3 p59 + X x0 = belief.mean(); + Matrix e0 = belief.covariance(); + + Z expected_z = sensor_model.getMean(x0); + Matrix q = sensor_model.getError(observation); + + Matrix h = sensor_model.getJacobian(x0); + Matrix ht = h.transpose(); + + Matrix i = Matrix::identity(e0.order()); + + Matrix e0_ht = e0 * ht; + Matrix k = e0_ht * (h * e0_ht + q).inverse(); + Vector v = (x0 + k * (observation - expected_z)); + X x1; + x1.set(std::move(v)); + Matrix e1 = (i - k * h) * e0; + + return std::make_shared>(x1, e1); + } + }; + } + } +} + + +#endif // EXTENDED_KALMAN_FILTER_HPP diff --git a/core/navigation/localization/include/localization/gaussian_action_model.hpp b/core/navigation/localization/include/localization/gaussian_action_model.hpp new file mode 100644 index 0000000..7d4a23b --- /dev/null +++ b/core/navigation/localization/include/localization/gaussian_action_model.hpp @@ -0,0 +1,31 @@ +#ifndef GAUSSIAN_ACTION_MODEL_HPP +#define GAUSSIAN_ACTION_MODEL_HPP + +#include "probability/data/matrix.hpp" +#include "action_model.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class GaussianActionModel : public ActionModel + { + using probability::Matrix; + public: + GaussianActionModel() { + } + virtual ~GaussianActionModel() {} + + // Expected next state. + virtual X getMean(const U& action, const X& state) const = 0; + // Process noise. + virtual Matrix getError(const U& action, const X& mean) const = 0; + }; + } + } +} + +#endif // GAUSSIAN_ACTION_MODEL_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp b/core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp new file mode 100644 index 0000000..f4d0734 --- /dev/null +++ b/core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp @@ -0,0 +1,27 @@ +#ifndef GAUSSIAN_SENSOR_MODEL_HPP +#define GAUSSIAN_SENSOR_MODEL_HPP + +#include "probability/data/matrix.hpp" +#include "sensor_model.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class GaussianSensorModel : public SensorModel + { + using probability::Matrix; + public: + GaussianSensorModel() {} + virtual ~GaussianSensorModel() override {} + + virtual Z getMean(const X& state) const = 0; + virtual Matrix getError(const Z& observation) const = 0; + }; + } + } +} +#endif // GAUSSIAN_SENSOR_MODEL_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/information_filter.hpp.hpp b/core/navigation/localization/include/localization/information_filter.hpp.hpp new file mode 100644 index 0000000..40b749d --- /dev/null +++ b/core/navigation/localization/include/localization/information_filter.hpp.hpp @@ -0,0 +1,70 @@ +#ifndef INFORMATION_FILTER_HPP +#define INFORMATION_FILTER_HPP + +#include "probability/continuous/gaussian_canonical.hpp" +#include "probability/data/matrix.hpp" +#include "probability/data/vector.hpp" +#include "kalman_filter.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class InformationFilter + { + using probability::GaussianCanonical; + using probability::Matrix; + using probability::Vector; + public: + InformationFilter() {} + virtual ~InformationFilter() {} + + std::shared_ptr> marginalize( + const KalmanActionModel& model, const U& action, + const GaussianCanonical& belief) + { + const Vector& xi = belief.information(); + const Matrix& A = model.a(); + const Matrix& B = model.b(); + const Vector& C = model.c(); + const Matrix& R = model.r(); + Matrix At = A.transpose(); + Matrix Sigma = belief.inversePrecision(); + Matrix ASigma = A * Sigma; + + Matrix nextOmega = (ASigma * At + R).inverse(); + X nextXi; + nextXi.set(nextOmega * (ASigma * xi + B * action + C)); + + return std::make_shared>(std::move(nextXi), std::move(nextOmega)); + } + + std::shared_ptr> bayesianInference( + const KalmanSensorModel& model, + const Z& data, + const GaussianCanonical& belief) + { + + const Vector& Xi = belief.information(); + const Matrix& Omega = belief.precision(); + const Matrix& C = model.c(); + Matrix Ct = C.transpose(); + const Matrix Q = model.q(); + const Matrix QInv = Q.inverse(); + const Matrix CtQInv = Ct * QInv; + + Matrix nextOmega = CtQInv * C + Omega; + Vector nextXi = CtQInv * data + Xi; + + return std::make_shared>(std::move(nextXi), std::move(nextOmega)); + } + }; + + } + } +} + +#endif // INFORMATION_FILTER_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/kalman_filter.hpp b/core/navigation/localization/include/localization/kalman_filter.hpp new file mode 100644 index 0000000..7c83485 --- /dev/null +++ b/core/navigation/localization/include/localization/kalman_filter.hpp @@ -0,0 +1,234 @@ +#ifndef KALMAN_FILTER_HPP +#define KALMAN_FILTER_HPP + +#include "probability/data/vector.hpp" +#include "probability/data/matrix.hpp" +#include "gaussian_action_model.hpp" +#include "gaussian_sensor_model.hpp.hpp" +#include "probability/continuous/gaussian_moment.hpp" + + +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + using probability::Vector; + + template + class KalmanActionModel : public GaussianActionModel + { + public: + KalmanActionModel(int x_order, int u_order) + : m_a(x_order, x_order) + , m_b(x_order, u_order) + , m_c(x_order) + , m_r(x_order, x_order) + { + } + + virtual ~KalmanActionModel() override + { + } + + X getMean(const U& action, const X& state) const override + { + X result; + result.set((a() * state) + (b() * action) + c()); + return result; + } + + Matrix getError(const U&, const X&) const override + { + return m_r; + } + + const Matrix& a() const + { + // n x n + return m_a; + } + + const Matrix& b() const + { + // n x m + return m_b; + } + + const Vector& c() const + { + // n + return m_c; + } + + const Matrix& r() const + { + return m_r; + } + + Matrix& a() + { + return m_a; + } + + Matrix& b() + { + return m_b; + } + + Vector& c() + { + return m_c; + } + + Matrix& r() + { + return m_r; + } + + private: + Matrix m_a; + Matrix m_b; + Vector m_c; + Matrix m_r; + }; + + template + class KalmanSensorModel : public GaussianSensorModel{ + public: + KalmanSensorModel(int x_order, int z_order) + : m_c(x_order, z_order) + , m_d(x_order) + , m_q(z_order, z_order) { + } + virtual ~KalmanSensorModel() override {} + + double conditionalProbabilityOf(const Z& observation, const X& state) const override + { + using probability::GaussianMoment; + // TODO: Gaussian moment caches a bunch of stuff. I should store one here + // instead of generating and solving one each time. + return GaussianMoment(getMean(state), getError(observation)).probabilityOf(observation); + } + + Z getMean(const X& state) const override + { + Z z; + z.set(m_c * state + m_d); + return z; + } + + Matrix getError(const Z& ) const override + { + return m_q; + } + + const Matrix& c() const + { + return m_c; + } + + const Vector& d() const + { + return m_d; + } + + const Matrix& q() const + { + return m_q; + } + + Matrix& c() + { + return m_c; + } + + Vector& d() + { + return m_d; + } + + Matrix& q() + { + return m_q; + } + + private: + Matrix m_c; + Vector m_d; + Matrix m_q; + }; + + template + class KalmanFilter + { + using probability::GaussianMoment; + using probability::Matrix; + public: + KalmanFilter() {} + + virtual ~KalmanFilter() {} + + std::shared_ptr> bayesianInference( + const KalmanSensorModel& model, + const Z& data, + const GaussianMoment& belief) const + { + // Table 3.1 p42 + auto& z = data; + auto& mu0 = belief.mean(); + auto& sigma0 = belief.covariance(); + + auto z_est = model.getMean(mu0); + auto Q = model.getError(z); + + Matrix C = model.c(); + Matrix K; + { + Matrix sc = sigma0 * C.transpose(); + K = sc * ((C * sc) + Q).inverse(); + } + + X mu1; + mu1.Set(mu0 + K * (z - z_est)); + + Matrix I = Matrix::identity(sigma0.rows()); + Matrix sigma1 = (I - K * C) * sigma0; + + return std::make_shared>(mu1, sigma1); + } + + std::shared_ptr> marginalize(const KalmanActionModel& model, + const U& action, + const GaussianMoment& state) const + { + // Table 3.1 p42 + const X& x1 = model.getMean(action, state.mean()); // 2 + Matrix q = model.a() * state.covariance() * model.a().Transpose(); //2a + q += model.getError(action, state.mean()); // 2b + return std::make_shared>(std::move(x1), std::move(q)); + } + + std::shared_ptr> marginalize(const KalmanActionModel& model, + const GaussianMoment& action, + const GaussianMoment& state) const + { + auto x1 = marginalize(model, action.mean(), state); + const Matrix& A = model.a(); + const Matrix& B = model.b(); + Matrix q = A * state.covariance() * A.transpose(); + q += (B * action.covariance() * B.transpose()); + q += x1->covariance(); + return std::make_shared>(x1->mean(), std::move(q)); + } + + }; + } + } +} + +#endif // KALMAN_FILTER_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/markov_action_chain.hpp b/core/navigation/localization/include/localization/markov_action_chain.hpp new file mode 100644 index 0000000..a9018cd --- /dev/null +++ b/core/navigation/localization/include/localization/markov_action_chain.hpp @@ -0,0 +1,182 @@ +#ifndef MARKOV_ACTION_CHAIN_HPP +#define MARKOV_ACTION_CHAIN_HPP + +#include "localization/probability/discrete/discrete_conditional.hpp" +#include "localization/probability/discrete/distribution_value_map.hpp" + +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class MarkovActionChain : pulbic probability::DiscreteConditional> + { + public: + typedef std::pair XU; + + MarkovActionChain() + { + } + + virtual ~MarkovActionChain() override + { + } + + std::vector domain() const override + { + std::vector result; + result.reserve(m_x.size()); + for (auto& x_x : m_x) + { + result.push_back(x_x.first); + } + return result; + } + + std::vector range() const override + { + std::vector result; + result.reserve(xu_.size()); + for (auto& xu_xu : m_xu) + { + result.push_back(xu_xu.first); + } + return result; + } + + double conditionalProbabilityOf(const X& x, const XU& xu) const override + { + const X& start = xu.first; + const U& u = xu.second; + auto start_action_map = m_p.find(start); + if (start_action_map == m_p.end()) + { + return 0.0; + } + auto action_end_map = start_action_map->second.find(u); + if (action_end_map == start_action_map->second.end()) + { + return 0.0; + } + auto end_value = action_end_map->second.find(x); + if (end_value == action_end_map->second.end()) + { + return 0.0; + } + return end_value->second; + } + + void set(X end, U action, X start, double p) + { + auto start_action_map = m_p.find(start); + if (start_action_map == m_p.end()) + { + start_action_map = m_p.emplace(start, std::map>()).first; + } + auto action_end_map = start_action_map->second.find(action); + if (action_end_map == start_action_map->second.end()) + { + auto emplace_result = start_action_map->second.emplace(action, std::map()); + action_end_map = emplace_result.first; + } + action_end_map->second[end] = p; + m_x[end] = end; + m_u[action] = action; + m_xu[XU(start, action)] = XU(start, action); + } + + double sumCondition(const XU& start_action) const + { + double sum = 0.0; + const X& start = start_action.first; + const U& action = start_action.second; + auto start_action_map = m_p.find(start); + if (start_action_map != m_p.end()) + { + auto action_end_map = start_action_map->second.find(action); + if (action_end_map != start_action_map->second.end()) + { + for (auto& end_value : action_end_map->second) + { + sum += end_value.second; + } + } + } + return sum; + } + + X sampleCondition(const XU& start_action, std::default_random_engine* generator) const + { + double sum = sumLikelihood(start_action); + if (sum == 0.0) + { + throw std::runtime_error("Conditional distribution is not conditioned on start_action. Cannot sample."); + } + std::uniform_real_distribution random(0, sum); + return sampleCondition(start_action, random(*generator)); + } + + X sampleCondition(const XU& start_action, double p) const + { + const X& start = start_action.first; + const U& action = start_action.second; + auto start_action_map = m_p.find(start); + if (start_action_map == m_p.end()) + { + throw std::runtime_error("MarkovActionChain does not have start state. Cannot sample."); + } + auto action_end_map = start_action_map->second.find(action); + if (action_end_map == start_action_map->second.end()) + { + throw std::runtime_error("MarkovActionChain does not have action from start state. Cannot sample."); + } + for (auto& end_value : action_end_map->second) + { + p -= end_value.second; + if (p <= 0) + { + return end_value.first; + } + } + throw std::runtime_error("End state for specified is badly specified. Cannot sample."); + } + + std::shared_ptr> marginalize(const probability::RandomDistribution& start_action) + { + auto result = std::make_shared > (); + for (auto& level0 : m_p) + { + X start = level0.first; + for (auto& level1 : level0.second) + { + U action = level1.first; + for (auto& level2 : level1.second) + { + X end = level2.first; + double p0 = level2.second; + XU xu(start, action); + double p1 = start_action.probabilityOf(xu); + double current_end_p = result->probabilityOf(end); + result->set(end, p0 * p1 + current_end_p); + } + } + } + result->normalize(); + return result; + } + + private: + std::map>> m_p; + std::map m_x; + std::map m_u; + std::map m_xu; + }; + } + } +} + +#endif // MARKOV_ACTION_CHAIN_HPP diff --git a/core/navigation/localization/include/localization/probability/bayes_distribution.hpp b/core/navigation/localization/include/localization/probability/bayes_distribution.hpp new file mode 100644 index 0000000..2194460 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/bayes_distribution.hpp @@ -0,0 +1,95 @@ +#ifndef BAYES_DISTRIBUTION_HPP +#define BAYES_DISTRIBUTION_HPP + +#include +#include + +#include "random_distribution.hpp" +#include "random_conditional.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class BayesDistribution : public RandomDistribution, + public std::enable_shared_from_this> + { + public: + BayesDistribution(std::shared_ptr> prior, + std::shared_ptr> sensor_model, + Y observation) + : m_prior(prior) + , m_sensor_model(sensor_model) + , m_data(std::move(observation)) + {} + + virtual ~BayesDistribution() {} + + double probabilityOf(const X& x) const override + { + double pyx = m_sensor_model->conditionalProbabilityOf(m_data, x); + double px = m_prior->probabilityOf(x); + double py = m_normalizer; + return pyx * px / py; + } + + X sample(std::default_random_engine*) const override + { + throw std::runtime_error("Not Implemented"); + } + + std::shared_ptr> chainObservation(Y y) + { + return std::make_shared< BayesDistribution >(this->shared_from_this(), m_sensor_model, std::move(y)); + } + + double estimateNormalizer(const std::vector& domain, double dx) + { + double sum = 0.0; + for (const X& x : domain) + { + double pz_given_x = m_sensor_model->conditionalProbabilityOf(m_data, x); + double px = m_prior->probabilityOf(x); + sum += pz_given_x * px; + } + m_normalizer = sum * dx; + return m_normalizer; + } + + const RandomConditional& sensorModel() const + { + return *m_sensor_model; + } + + const Y& data() const + { + return m_data; + } + + double& normalizer() + { + return m_normalizer; + } + + double normalizer() const + { + return m_normalizer; + } + + private: + std::shared_ptr> m_prior; + std::shared_ptr> m_sensor_model; + Y m_data; + double m_normalizer = 1.0; + }; + } + } + } +} + +#endif // BAYES_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/probability/conditional_map.hpp b/core/navigation/localization/include/localization/probability/conditional_map.hpp new file mode 100644 index 0000000..36fa096 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/conditional_map.hpp @@ -0,0 +1,52 @@ +#ifndef CONDITIONAL_MAP_HPP +#define CONDITIONAL_MAP_HPP + +#include +#include + +#include "random_distribution.hpp" +#include "random_conditional.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class ConditionalMap : public RandomConditional + { + public: + ConditionalMap() + {} + + virtual ~ConditionalMap() + {} + + void set(Y y, std::shared_ptr> x) + { + m_map[y] = x; + } + + double conditionalProbabilityOf(const X& x, const Y& y) const override + { + double result = 0.0; + auto key_value = m_map.find(y); + if (key_value != m_map.end()) + { + result = key_value->second->probabilityOf(x); + } + return result; + } + + private: + std::map>> m_map; + }; + } + } + } +} + +#endif // CONDITIONAL_MAP_HPP diff --git a/core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp b/core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp new file mode 100644 index 0000000..4cd05f6 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp @@ -0,0 +1,134 @@ +#ifndef GAUSSIAN_CANONICAL_HPP +#define GAUSSIAN_CANONICAL_HPP + +#include +#include + +#include "../random_distribution.hpp" +#include "../data/vector.hpp" +#include "../data/matrix.hpp" +#include "gaussian_moment.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class GaussianCanonical : public RandomDistribution + { + public: + GaussianCanonical(const Vector& xi, const Matrix& omega) : + m_information(std::move(xi)), + m_precision(std::move(omega)), + m_sigma(omega.rows(), omega.cols()), + m_mu() + { + } + + explicit GaussianCanonical(const GaussianMoment& x) : + m_information(x.covariance().Inverse()), + m_precision(m_information * x.mean()), + m_mu(x.mean()), + m_sigma(x.covariance()), + m_is_sigma_valid(true), + m_is_mu_valid(true) + { + } + GaussianCanonical(GaussianCanonical&& that) : + m_precision(std::move(that.m_precision)), + m_information(std::move(that.m_information)), + m_is_sigma_valid(that.m_is_sigma_valid), + m_is_mu_valid(that.m_is_mu_valid), + m_sigma(std::move(that.m_sigma)), + m_mu(std::move(that.m_mu)) + { + } + + GaussianCanonical& operator=(GaussianCanonical&& that) + { + this->m_precision = std::move(that.m_precision); + this->m_information = std::move(that.m_information); + this->m_is_sigma_valid = that.m_is_sigma_valid; + this->m_is_mu_valid = that.m_is_mu_valid; + this->m_sigma = std::move(that.m_sigma); + this->m_mu = std::move(that.m_mu); + return *this; + } + + virtual ~GaussianCanonical() override + { + } + + const Vector& information() const + { + return m_information; + } + const Matrix& precision() const + { + return m_precision; + } + const Matrix& sigma() const + { + return inversePrecision(); + } + const Vector& mu() const + { + return getMu(); + } + + const Matrix& inversePrecision() const + { + if (!m_is_sigma_valid) + { + m_sigma = precision().inverse(); + m_is_sigma_valid = true; + } + return m_sigma; + } + + const X& getMu() const + { + if (!m_is_mu_valid) + { + m_mu.set(sigma() * information()); + m_is_mu_valid = true; + } + return m_mu; + } + + double probabilityOf(const X&) const override + { + throw std::runtime_error("GaussianCanonical::ProbabilityOf not implemented."); + } + + X sample(std::default_random_engine*) const override + { + throw std::runtime_error("GaussianCanonical::Sample not implemented."); + } + + private: + Vector m_information; // Omega + Matrix m_precision; // xi + mutable bool m_is_sigma_valid = false; + mutable bool m_is_mu_valid = false; + mutable Matrix m_sigma; + mutable X m_mu; + + }; + + template + static std::ostream& operator<<(std::ostream& os, const GaussianCanonical& x) + { + os << "omega=" << x.information() << ", xi=" << x.precision(); + return os; + }; + } + } + } +} + +#endif // GAUSSIAN_CANONICAL_HPP diff --git a/core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp b/core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp new file mode 100644 index 0000000..f367e96 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp @@ -0,0 +1,132 @@ +#ifndef GAUSSIAN_MOMENT_HPP +#define GAUSSIAN_MOMENT_HPP + +#include +#include +#include + +#include "../random_distribution.hpp" +#include "../data/vector.hpp" +#include "../data/matrix.hpp" +#include "../data/matrix_vector_operators.hpp" + + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class GaussianMoment : public RandomDistribution + { + public: + static double fn(double x, double mean, double variance) + { + double A = sqrt(1.0 / (2 * M_PI * variance)); + double dx = x - mean; + double x2 = dx * dx; + double e = exp(-0.5 * x2 / variance); + return A * e; + } + static double fn(const Vector& x, const Vector& mean, const Matrix& covariance, const Matrix& inverse_covariance) + { + double A = sqrt(1.0 / ((2 * M_PI) * covariance).determinant()); + Vector dx = x - mean; + double e = exp(-(dx * inverse_covariance * dx)); + return A * e; + } + static double fn(const Vector& x, const Vector& mean, const Matrix& covariance) + { + return fn(x, mean, covariance, covariance.inverse()); + } + + GaussianMoment(X mean, Matrix covariance) + : m_mean(std::move(mean)) + , m_covariance(std::move(covariance)) + { + } + + GaussianMoment(GaussianMoment&& that) + : m_mean(std::move(that.m_mean)) + , m_covariance(std::move(that.m_covariance)) + { + } + + GaussianMoment& operator=(GaussianMoment&& that) { + this->m_mean = std::move(that.m_mean); + this->m_covariance = std::move(that.m_covariance); + this->m_inverse_covariance = std::move(that.m_inverse_covariance); + this->m_sqrt_covariance = std::move(that.m_sqrt_covariance); + this->m_is_valid_inverse = m_is_valid_inverse; + this->m_is_valid_sqrt = m_is_valid_sqrt; + return *this; + } + + virtual ~GaussianMoment() + { + } + + double probabilityOf(const X& x) const override + { + return fn(x.aliasVector(), mean().aliasVector(), covariance(), inverseCovariance()); + } + + X sample(std::default_random_engine* generator) const override { + std::normal_distribution n01(0, 1); + Vector sample(order()); + for (std::size_t i = 0; i < sample.order(); i++) { + // need a N(0,1) here + sample[i] = n01(*generator); + } + X result; + result.set(mean() + sqrtCovariance() * sample); + return result; + } + + std::size_t order() const { + return mean().order(); + } + + const X& mean() const { + return m_mean; + } + + const Matrix& covariance() const { + return m_covariance; + } + + const Matrix& inverseCovariance() const { + if (!m_is_valid_inverse) { + m_inverse_covariance = m_covariance.inverse(); + m_is_valid_inverse = true; + } + return m_inverse_covariance; + } + + const Matrix& sqrtCovariance() const { + if (!m_is_valid_sqrt) { + m_sqrt_covariance = covariance().sqrt(); + m_is_valid_sqrt = true; + } + return m_sqrt_covariance; + } + + private: + X m_mean; + Matrix m_covariance; + // These values are mutable because they are cached derivations of covariance + mutable Matrix m_inverse_covariance; + mutable Matrix m_sqrt_covariance; + mutable bool m_is_valid_inverse = false; + mutable bool m_is_valid_sqrt = false; + }; + } + } + } +} + + +#endif // GAUSSIAN_MOMENT_HPP diff --git a/core/navigation/localization/include/localization/probability/data/matrix.hpp b/core/navigation/localization/include/localization/probability/data/matrix.hpp new file mode 100644 index 0000000..5ca9058 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/data/matrix.hpp @@ -0,0 +1,81 @@ +#ifndef MATRIX_HPP +#define MATRIX_HPP + +#include +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + + class Vector; + + class Matrix + { + public: + static Matrix identity(size_t order) + { + std::vector a(order * order); + for (std::size_t i = 0; i < a.size(); i += order + 1) + { + a[i] = 1.0; + } + return Matrix(order, order, a); + } + + Matrix(); + Matrix(std::size_t rows, std::size_t cols); + Matrix(std::size_t rows, std::size_t cols, std::vector values); + Matrix(std::size_t rows, std::size_t cols, std::vector values, bool row_based); + Matrix(const Matrix& that); + Matrix(Matrix&& that); + + virtual ~Matrix(); + + std::size_t rows() const; + std::size_t cols() const; + std::size_t order() const; + double at(std::size_t row, std::size_t col) const; + Vector column(std::size_t col) const; + Matrix& operator=(Matrix&& that); + Matrix& operator+=(const Matrix& that); + Matrix operator+(const Matrix& that) const; + Matrix operator-(const Matrix& that) const; + Matrix operator*(const Matrix& that) const; + Matrix transpose() const; + Matrix inverse() const; + Matrix sqrt() const; + double determinant() const; + void scaleRow(std::size_t r, double k); + void combineRow(std::size_t srcRow, std::size_t dstRow, double k); + Matrix luDecompositionByGE(int* p_swap_count, std::vector* p_pivot) const; + Matrix roundSymmetry(); + + private: + std::size_t getIndex(std::size_t row, std::size_t col) const; + static void initPivots(std::size_t n, std::vector* p_pivot); + double partialPivot(std::vector* p_pivot, std::size_t k, int* p_swap_count) const; + bool isSymmetric() const; + Matrix choleskyDecomposition() const; + std::size_t m_rows; + std::size_t m_cols; + std::vector m_m; + bool m_row_based = true; + + friend Matrix operator*(double scalar, const Matrix& m); + }; + + std::ostream& operator<<(std::ostream& os, const Matrix& v); + } + } + } +} + +#endif // MATRIX_HPP diff --git a/core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp b/core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp new file mode 100644 index 0000000..04d10c7 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp @@ -0,0 +1,25 @@ +#ifndef MATRIX_VECTOR_OPERATORS_HPP +#define MATRIX_VECTOR_OPERATORS_HPP + +#include "localization/probability/data/matrix.hpp" +#include "localization/probability/data/vector.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + Vector operator*(const Matrix& m, const Vector& v); + Vector operator*(const Vector& v, const Matrix& m); + Matrix operator*(double scalar, const Matrix& m); + double operator*(const Vector& v1, const Vector& v2); + } + } + } +} + + +#endif // MATRIX_VECTOR_OPERATORS_HPP diff --git a/core/navigation/localization/include/localization/probability/data/vector.hpp b/core/navigation/localization/include/localization/probability/data/vector.hpp new file mode 100644 index 0000000..46ad24a --- /dev/null +++ b/core/navigation/localization/include/localization/probability/data/vector.hpp @@ -0,0 +1,73 @@ +#ifndef VECTOR_HPP +#define VECTOR_HPP + +#include +#include +#include +#include +#include +#include +#include + +#include "localization/probability/data/matrix.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + + class Vector + { + public: + Vector(); + explicit Vector(std::vector a); + explicit Vector(const Vector& v); + Vector(Vector&& v); + explicit Vector(std::size_t order); + virtual ~Vector(); + std::size_t order() const; + double& at(int i); + double at(int i) const; + double& operator[](std::size_t i); + double operator[](std::size_t i) const; + Matrix cross(const Vector& that) const; + Vector& operator=(const Vector& that); + virtual void set(Vector a); + void copyAssign(const Vector& v); + void moveAssign(Vector&& v); + Vector aliasVector() const; + double dot(const Vector& v2) const; + Vector operator+(const Vector& v2) const; + Vector operator-(const Vector& v2) const; + bool operator<(const Vector& v2) const; + bool operator>(const Vector& v2) const; + bool operator==(const Vector& v) const; + protected: + + private: + std::vector m_a; + }; + + std::ostream& operator<<(std::ostream& os, const Vector& v); + + +#define DEFINE_VECTOR1(X) \ +class X : public ::rip::navigation::localization::probability::Vector { \ +public: \ + X() : Vector(std::vector({ 0.0 })) {} \ + X(double x) : Vector(std::vector({ x })) {} \ + X(const X& v) : Vector(v) {} \ + X(const X&& v) : Vector(v) {} \ + X(::rip::navigation::localization::probability::Vector&& v) : Vector(v) {} \ + X& operator=(const X& that) { Vector::operator=(that); return *this; } \ +} + } + } + } +} + +#endif // VECTOR_HPP diff --git a/core/navigation/localization/include/localization/probability/data/vector_range.hpp b/core/navigation/localization/include/localization/probability/data/vector_range.hpp new file mode 100644 index 0000000..0fd49b9 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/data/vector_range.hpp @@ -0,0 +1,64 @@ +#ifndef VECTOR_RANGE_HPP +#define VECTOR_RANGE_HPP + +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class VectorRange + { + public: + VectorRange(X min, X max) + : m_min(std::move(min)), m_max(std::move(max)) + { + } + virtual ~VectorRange() + { + } + + double area() const + { + Vector dx = m_max - m_min; + double result = 1.0; + for (unsigned int i = 0; i < dx.order(); i++) + { + result *= dx[i]; + } + return fabs(result); + } + + bool contains(const X& x) const + { + return x > m_min && x < m_max; + } + + X sample(std::default_random_engine* generator) const + { + X result; + for (unsigned int i = 0; i < result.order(); i++) + { + std::uniform_real_distribution random(m_min[i], m_max[i]); + result[i] = random(*generator); + } + return result; + } + + private: + X m_min; + X m_max; + }; + } + } + } +} + +#endif // VECTOR_RANGE_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp b/core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp new file mode 100644 index 0000000..5af366a --- /dev/null +++ b/core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp @@ -0,0 +1,170 @@ +#ifndef CONDITIONAL_VALUE_MAP_HPP +#define CONDITIONAL_VALUE_MAP_HPP + +#include +#include +#include +#include +#include + +#include "discrete_conditional.hpp" +#include "distribution_value_map.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class ConditionalValueMap : public DiscreteConditional + { + public: + ConditionalValueMap() {} + + virtual ~ConditionalValueMap() override {} + + virtual std::vector domain() const override + { + std::vector keys; + std::transform( + m_domain.begin(), + m_domain.end(), + std::back_inserter(keys), + [](const std::pair& pair){return pair.first;}); + return keys; + /* + std::vector result; + result.reserve(m_domain.size()); + for (auto& pair : m_domain) + { + result.push_back(pair.first); + } + return result; + */ + } + + virtual std::vector range() const override + { + std::vector result; + result.reserve(m_map.size()); + for (auto& pair : m_map) + { + result.push_back(pair.first); + } + return result; + } + + double conditionalProbabilityOf(const X& x, const Y& y) const override + { + auto y_map = m_map.find(y); + if (y_map == m_map.end()) + { + // todo: Does zero make sense here? Does an unknown imply zero change + return 0.0; + } + + auto x_value = y_map->second.find(x); + if (x_value == y_map->second.end()) + { + return 0.0; + } + return x_value->second; + } + + void set(const X& x, const Y& y, double p) + { + auto value_map = m_map.find(y); + if (value_map == m_map.end()) + { + value_map = m_map.emplace(y, std::map()).first; + } + value_map->second[x] = p; + m_domain[x] = x; + } + + std::shared_ptr> likelihoodOf(const X& data) const + { + auto result = std::make_shared>(); + for (auto& y_map : m_map) + { + auto x_value = y_map.second.find(data); + if (x_value != y_map.second.end()) + { + result->set(y_map.first, x_value->second); + } + } + return result; + } + + std::shared_ptr> bayesianInference(X data, const DistributionValueMap& prior) const + { + auto likelihood = likelihoodOf(data); + return likelihood->product(prior); + } + + double sumLikelihood(const X& x) const + { + double sum = 0.0; + for (auto& y_map : m_map) + { + auto x_value = y_map.second.find(x); + if (x_value != y_map.second.end()) + { + sum += x_value->second; + } + } + return sum; + } + + Y sampleLikelihood(const X& x, double p) const + { + for (auto& y_map : m_map) + { + Y y = y_map.first; + auto x_value = y_map.second.find(x); + if (x_value != y_map.second.end()) + { + p -= x_value->second; + if (p < 0) + { + return y; + } + } + } + throw std::runtime_error("Conditional distribution is not conditioned on x. Cannot sample."); + } + + template + std::shared_ptr> marginalize(const DiscreteConditional& that) + { + auto result = std::make_shared>(); + for (X& x : this->domain()) + { + for (Z& z : that.range()) + { + double p = 0.0; + for (Y& y : that.domain()) + { + double xy = this->conditionalProbabilityOf(x, y); + double yz = that.conditionalProbabilityOf(y, z); + p += xy * yz; + } + result->set(x, z, p); + } + } + return result; + } + + private: + std::map> m_map; + std::map m_domain; + }; + } + } + } +} + +#endif // CONDITIONAL_VALUE_MAP_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp b/core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp new file mode 100644 index 0000000..ced4b58 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp @@ -0,0 +1,36 @@ +#ifndef DISCRETE_CONDITIONAL_HPP +#define DISCRETE_CONDITIONAL_HPP + +#include +#include + +#include "../random_conditional.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + + template + class DiscreteConditional : public RandomConditional + { + public: + DiscreteConditional() {} + + virtual ~DiscreteConditional() override {} + + virtual std::vector domain() const = 0; + + virtual std::vector range() const = 0; + }; + + } + } + } +} + +#endif // DISCRETE_CONDITIONAL_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp b/core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp new file mode 100644 index 0000000..b6e7b33 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp @@ -0,0 +1,48 @@ +#ifndef DISCRETE_DISTRIBUTION_HPP +#define DISCRETE_DISTRIBUTION_HPP + +#include + +#include "../random_distribution.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class DiscreteDistribution : public RandomDistribution + { + public: + DiscreteDistribution() {} + + virtual ~DiscreteDistribution() {} + + protected: + template + static X selectFromMap(const std::map& map, double select) + { + if (map.empty()) + { + throw std::runtime_error("Cannot sample from an empty distribution"); + } + X result; + auto p = map.begin(); + do + { + result = p->first; + select -= p->second; + } + while (++p != map.end() && select >= 0); + return result; + } + }; + } + } + } +} + +#endif // DISCRETE_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp b/core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp new file mode 100644 index 0000000..7438c84 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp @@ -0,0 +1,65 @@ +#ifndef DISTRIBUTION_COUNTER_HPP +#define DISTRIBUTION_COUNTER_HPP + +#include +#include + +#include "discrete_distribution.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class DistributionCounter : public DiscreteDistribution + { + public: + DistributionCounter() {} + + virtual ~DistributionCounter() {} + + double probabilityOf(const X& x) const override + { + double result = 0.0; + auto key_value = m_map.find(x); + if (key_value != m_map.end()) + { + double n = key_value->second; + result = n / m_total; + } + return result; + } + + X sample(std::default_random_engine* generator) const override + { + std::uniform_real_distribution random(0, m_total); + double select = random(*generator); + return this->selectFromMap(m_map, select); + } + + void addObservation(X x) + { + int n = 0; + auto key_value = m_map.find(x); + if (key_value != m_map.end()) + { + n = key_value->second; + } + m_map[x] = n + 1; + m_total ++; + } + + private: + int m_total = 0; + std::map m_map; + }; + } + } + } +} + +#endif // DISTRIBUTION_COUNTER_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp b/core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp new file mode 100644 index 0000000..ecc1b37 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp @@ -0,0 +1,118 @@ +#ifndef DISTRIBUTION_VALUE_MAP_HPP +#define DISTRIBUTION_VALUE_MAP_HPP + +#include +#include +#include +#include + +#include "discrete_distribution.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class DistributionValueMap : public DiscreteDistribution + { + public: + DistributionValueMap() {} + + DistributionValueMap(X x0, double p0, X x1, double p1) + { + set(x0, p0); + set(x1, p1); + normalize(); + } + + DistributionValueMap(const std::vector& list) + { + for (auto& x : list) + { + set(x, 1.0); + } + normalize(); + } + + virtual ~DistributionValueMap() override {} + + double probabilityOf(const X& x) const override + { + double result = 0.0; + auto it = m_map.find(x); + if (it != m_map.end()) + { + result = it->second; + } + return result; + } + + X sample(std::default_random_engine* generator) const override + { + std::uniform_real_distribution random(0, 1); + double select = random(*generator); + return this->selectFromMap(m_map, select); + } + + void set(X x, double p) + { + m_map[x] = p; + } + + void normalize() + { + double sum = 0.0; + for (auto& pair : m_map) + { + sum += pair.second; + } + for (auto& pair : m_map) + { + pair.second /= sum; + } + } + + double entropy() const + { + double result = 0.0; + for (auto& pair : m_map) + { + double p = pair.second; + if (p != 0) + { + result -= p * log2(p); + } + } + return result; + } + + std::shared_ptr> product(const DistributionValueMap& that) const + { + auto result = std::make_shared>(); + for (auto& pair : m_map) + { + double lp = pair.second; + double rp = that.probabilityOf(pair.first); + double p = lp * rp; + if (p != 0) + { + result->set(pair.first, p); + } + } + result->normalize(); + return result; + } + + private: + std::map m_map; + }; + } + } + } +} + +#endif // DISTRIBUTION_VALUE_MAP_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp b/core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp new file mode 100644 index 0000000..e003718 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp @@ -0,0 +1,158 @@ +#ifndef MARKOV_CHAIN_HPP +#define MARKOV_CHAIN_HPP + +#include +#include + +#include "discrete_conditional.hpp" +#include "distribution_value_map.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class MarkovChain : public DiscreteConditional + { + public: + MarkovChain() {} + + virtual ~MarkovChain() override {} + + std::vector domain() const override + { + std::vector result; + result.reserve(m_range.size()); + for (auto& pair : m_range) + { + result.push_back(pair.first); + } + return result; + } + + std::vector range() const override + { + std::vector result; + result.reserve(m_transitions.size()); + for (auto& pair : m_transitions) + { + result.push_back(pair.first); + } + return result; + } + + double conditionalProbabilityOf(const X& end, const X& start) const override + { + auto end_map = m_transitions.find(start); + if (end_map == m_transitions.end()) + { + // todo: Does zero make sense here? Does an unknown imply zero chance? + return 0.0; + } + auto result = end_map->second.find(end); + if (result == end_map->second.end()) + { + return 0.0; + } + return result->second; + } + + void set(const X& end, const X& start, double p) + { + auto end_map = m_transitions.find(start); + if (end_map == m_transitions.end()) + { + end_map = m_transitions.emplace(start, std::map()).first; + } + end_map->second[end] = p; + m_range[end] = end; + } + + X doTransition(const X& start, std::default_random_engine* generator) + { + std::uniform_real_distribution dist(0, 1); + double select = dist(*generator); + X result = start; + auto end_map = m_transitions.find(start); + if (end_map != m_transitions.end()) + { + for (auto& x : end_map->second) + { + select -= x.second; + if (select <= 0) + { + result = x.first; + break; + } + } + } + return result; + } + + std::shared_ptr reverse() const + { + auto result = std::make_shared>(); + for (X today : range()) + { + auto x = this->bayesianInference(today, DistributionValueMap(domain())); + for (X yesterday : domain()) + { + double p = x->probabilityOf(yesterday); + result->set(yesterday, today, p); + } + } + return result; + } + + std::shared_ptr> likelihoodOf(const X& data) const + { + auto result = std::make_shared>(); + for (auto& end_map : m_transitions) + { + auto end = end_map.second.find(data); + if (end != end_map.second.end()) + { + result->set(end_map.first, end->second); + } + } + return result; + } + + std::shared_ptr> marginalize(const RandomDistribution& start) const + { + auto result = std::make_shared>(); + for (auto& xfr_map : m_transitions) + { + double pstart = start.probabilityOf(xfr_map.first); + for (auto& end : xfr_map.second) + { + double pend = end.second; + double p0 = result->probabilityOf(end.first); + double pxfr = pend * pstart; + double p1 = p0 + pxfr; + result->set(end.first, p1); + } + } + result->normalize(); + return result; + } + + std::shared_ptr> bayesianInference(const X& data, const DistributionValueMap& prior) const + { + auto likelihood = likelihoodOf(data); + return likelihood->product(prior); + } + private: + std::map> m_transitions; + std::map m_range; + }; + } + } + } +} + +#endif // MARKOV_CHAIN_HPP diff --git a/core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp b/core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp new file mode 100644 index 0000000..042ab78 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp @@ -0,0 +1,54 @@ +#ifndef INDEPENDENT_PAIR_DISTRIBUTION_HPP +#define INDEPENDENT_PAIR_DISTRIBUTION_HPP + +#include +#include + +#include "random_distribution.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class IndependentPairDistribution : public RandomDistribution> + { + public: + typedef std::pair AB; + + IndependentPairDistribution(std::shared_ptr< RandomDistribution > a, std::shared_ptr< RandomDistribution > b) + : m_a(a) + , m_b(b) + { + + } + + virtual ~IndependentPairDistribution() + { + } + + double probabilityOf(const AB& t) const override + { + return m_a->probabilityOf(t.first) * m_b->probabilityOf(t.second); + } + + AB sample(std::default_random_engine* generator) const override + { + return AB(m_a->sample(generator), m_b->sample(generator)); + } + + private: + std::shared_ptr< RandomDistribution > m_a; + std::shared_ptr< RandomDistribution > m_b; + + }; + + } + } + } +} +#endif // INDEPENDENT_PAIR_DISTRIBUTION_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/probability/random_conditional.hpp b/core/navigation/localization/include/localization/probability/random_conditional.hpp new file mode 100644 index 0000000..c179a57 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/random_conditional.hpp @@ -0,0 +1,29 @@ +#ifndef RANDOM_CONDITIONAL_HPP +#define RANDOM_CONDITIONAL_HPP + +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class RandomConditional + { + public: + RandomConditional() {} + + virtual ~RandomConditional() {} + + virtual double conditionalProbabilityOf(const X& x, const Y& y) const = 0; + }; + } + } + } +} + +#endif // RANDOM_CONDITIONAL_HPP diff --git a/core/navigation/localization/include/localization/probability/random_distribution.hpp b/core/navigation/localization/include/localization/probability/random_distribution.hpp new file mode 100644 index 0000000..37588ac --- /dev/null +++ b/core/navigation/localization/include/localization/probability/random_distribution.hpp @@ -0,0 +1,31 @@ +#ifndef RANDOM_DISTRIBUTION_HPP +#define RANDOM_DISTRIBUTION_HPP + +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class RandomDistribution + { + public: + RandomDistribution() {} + + virtual ~RandomDistribution() {} + + virtual double probabilityOf(const X& x) const = 0; + + virtual X sample(std::default_random_engine* generator) const = 0; + }; + } + } + } +} + +#endif // RANDOM_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/probability/random_uniform.hpp b/core/navigation/localization/include/localization/probability/random_uniform.hpp new file mode 100644 index 0000000..fa5fd4e --- /dev/null +++ b/core/navigation/localization/include/localization/probability/random_uniform.hpp @@ -0,0 +1,54 @@ +#ifndef RANDOM_UNIFORM_HPP +#define RANDOM_UNIFORM_HPP + +#include "data/vector.hpp" +#include "data/vector_range.hpp" +#include "random_distribution.hpp" + +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class RangedUniform : public RandomDistribution + { + public: + RangedUniform(X min, X max) + : m_range(std::move(min), std; : move(max)) + { + static_assert(std::is_base_of::value, "X must derive from Vector"); + } + + ~RangedUniform() override {} + + double probabilityOf(const X& x) const override + { + if (m_range.contains(x)) + { + + return 1.0 / m_range.area(); + } + return 0.0; + } + + X sample(std::default_random_engine* generator) const override + { + return m_range.sample(generator); + } + + private: + VectorRange m_range; + }; + } + } + } +} + +#endif // RANDOM_UNIFORM_HPP diff --git a/core/navigation/localization/include/localization/probability/ranged_uniform.hpp b/core/navigation/localization/include/localization/probability/ranged_uniform.hpp new file mode 100644 index 0000000..ec80794 --- /dev/null +++ b/core/navigation/localization/include/localization/probability/ranged_uniform.hpp @@ -0,0 +1,51 @@ +#ifndef RANGED_UNIFORM_HPP +#define RANGED_UNIFORM_HPP + +#include "localization/probability/data/vector.hpp" +#include "localization/probability/data/vector_range.hpp" +#include "random_distribution.hpp" + +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + template + class RangedUniform : public RandomDistribution { + public: + RangedUniform(X min, X max) + : m_range(std::move(min), std::move(max)) + { + static_assert(std::is_base_of::value, "X must derive from Vector"); + } + + ~RangedUniform() override { + } + + double probabilityOf(const X& x) const override { + if (m_range.contains(x)) + { + return 1.0 / m_range.area(); + } + return 0.0; + } + + X sample(std::default_random_engine* generator) const override + { + return m_range.sample(generator); + } + + private: + VectorRange m_range; + }; + } + } + } +} +#endif // RANGED_UNIFORM_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/sensor_model.hpp b/core/navigation/localization/include/localization/sensor_model.hpp new file mode 100644 index 0000000..c0ac473 --- /dev/null +++ b/core/navigation/localization/include/localization/sensor_model.hpp @@ -0,0 +1,23 @@ +#ifndef SENSOR_MODEL_HPP +#define SENSOR_MODEL_HPP + +namespace rip +{ + namespace navigation + { + namespace localization + { + template + class SensorModel + { + public: + SensorModel() {} + virtual ~SensorModel() {} + + virtual double conditionalProbabilityOf(const Z& observation, const X& state) const = 0; + }; + } + } +} + +#endif // SENSOR_MODEL_HPP \ No newline at end of file diff --git a/core/navigation/localization/src/probability/data/matrix.cpp b/core/navigation/localization/src/probability/data/matrix.cpp new file mode 100644 index 0000000..2157228 --- /dev/null +++ b/core/navigation/localization/src/probability/data/matrix.cpp @@ -0,0 +1,524 @@ +#include "localization/probability/data/vector.hpp" +#include "localization/probability/data/matrix.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + Matrix::Matrix() + : m_rows(0) + , m_cols(0) + , m_m() + , m_row_based(true) + { + } + + Matrix::Matrix(std::size_t rows, std::size_t cols) + : m_rows(rows) + , m_cols(cols) + , m_m(rows * cols) + , m_row_based(true) + { + } + + Matrix::Matrix(std::size_t rows, std::size_t cols, std::vector values) + : m_rows(rows) + , m_cols(cols) + , m_m(values) + , m_row_based(true) + { + } + + Matrix::Matrix(std::size_t rows, std::size_t cols, std::vector values, bool row_based) + : m_rows(rows) + , m_cols(cols) + , m_m(values) + , m_row_based(row_based) + { + } + + Matrix::Matrix(const Matrix& that) + : m_rows(that.m_rows) + , m_cols(that.m_cols) + , m_m(that.m_m) + , m_row_based(that.m_row_based) + { + } + + Matrix::Matrix(Matrix&& that) + : m_rows(that.m_rows) + , m_cols(that.m_cols) + , m_m(std::move(that.m_m)) + , m_row_based(that.m_row_based) + { + } + + Matrix::~Matrix() + { + } + + std::size_t Matrix::rows() const + { + return m_rows; + } + + std::size_t Matrix::cols() const + { + return m_cols; + } + + std::size_t Matrix::order() const + { + if (m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + return m_rows; + } + + double Matrix::at(std::size_t row, std::size_t col) const + { + return m_m[getIndex(row, col)]; + } + + Vector Matrix::column(std::size_t col) const + { + Vector result; + std::vector a(m_rows); + if (m_row_based) + { + for (std::size_t i = 0; i < a.size(); i++) + { + a[i] = at(i, col); + } + } + else + { + std::size_t col_start = getIndex(0, col); + std::size_t col_end = col_start + a.size(); + std::copy(m_m.begin() + col_start, m_m.begin() + col_end, a.begin()); + } + return Vector(a); + } + + Matrix& Matrix::operator=(Matrix&& that) + { + m_rows = that.m_rows; + m_cols = that.m_cols; + m_m = std::move(that.m_m); + m_row_based = that.m_row_based; + return *this; + } + + Matrix& Matrix::operator+=(const Matrix& that) + { + if (m_rows != that.m_rows || m_cols != that.m_cols) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + + if (m_row_based == that.m_row_based) + { + for (std::size_t i = 0; i < m_m.size(); i++) + { + m_m[i] += that.m_m[i]; + } + } + else + { + for (std::size_t row = 0; row < rows(); row++) + { + for (std::size_t col = 0; col < cols(); col++) + { + std::size_t this_index = getIndex(row, col); + std::size_t that_index = that.getIndex(row, col); + m_m[this_index] += that.m_m[that_index]; + } + } + } + + return *this; + } + + Matrix Matrix::operator+(const Matrix& that) const + { + if (m_rows != that.m_rows || m_cols != that.m_cols || m_row_based != that.m_row_based) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + std::vector m(m_m.size()); + for (std::size_t i = 0; i < m_m.size(); i++) + { + m[i] = m_m[i] + that.m_m[i]; + } + return Matrix(m_rows, m_cols, m, m_row_based); + } + + Matrix Matrix::operator-(const Matrix& that) const + { + if (m_rows != that.m_rows || m_cols != that.m_cols || m_row_based != that.m_row_based) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + std::vector m(m_m.size()); + for (std::size_t i = 0; i < m_m.size(); i++) + { + m[i] = m_m[i] - that.m_m[i]; + } + return Matrix(m_rows, m_cols, m, m_row_based); + } + + Matrix Matrix::operator*(const Matrix& that) const + { + if (m_cols != that.m_rows) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + Matrix result(m_rows, that.m_cols); + for (std::size_t col = 0; col < that.m_cols; col++) + { + for (std::size_t row = 0; row < m_rows; row++) + { + for (std::size_t k = 0; k < m_cols; k++) + { + result.m_m[result.getIndex(row, col)] += at(row, k) * that.at(k, col); + } + } + } + return result; + } + + Matrix Matrix::transpose() const + { + return Matrix(m_cols, m_rows, m_m, !m_row_based); + } + + Matrix Matrix::inverse() const + { + if (m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + // set up mean as identity + Matrix result(m_rows, m_cols); + for (std::size_t i = 0; i < m_cols; i++) + { + result.m_m[result.getIndex(i, i)] = 1.0; + } + + Matrix working(m_rows, m_cols, m_m); // creates a copy of m_ + // Eliminate L + for (std::size_t col = 0; col < m_cols; col++) + { + double diag = working.at(col, col); + for (std::size_t row = col + 1; row < m_rows; row++) + { + double target = working.at(row, col); + double a = -target / diag; + working.combineRow(col, row, a); + result.combineRow(col, row, a); + } + working.scaleRow(col, 1.0 / diag); + result.scaleRow(col, 1.0 / diag); + } + // Eliminate U + for (std::size_t col = m_cols - 1; col >= 1; col--) + { + double diag = working.at(col, col); // 1.0 + for (std::size_t row_plus_one = col; row_plus_one > 0; row_plus_one--) + { + std::size_t row = row_plus_one - 1; + double target = working.at(row, col); + double a = -target / diag; + working.combineRow(col, row, a); + result.combineRow(col, row, a); + } + } + return result; + } + + Matrix Matrix::sqrt() const + { + // The matrix square root should be calculated using numerically efficient and stable methods such as the Cholesky decomposition + if (m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + return choleskyDecomposition(); + } + + double Matrix::determinant() const + { + if (rows() != cols()) + { + throw std::runtime_error("Matrix must be square."); + } + switch (rows()) + { + case 0: + return 1; + case 1: + return m_m[0]; + case 2: + { + return m_m[0] * m_m[3] - m_m[1] * m_m[2]; + } + case 3: + { + double a = at(0, 0); + double b = at(0, 1); + double c = at(0, 2); + double d = at(1, 0); + double e = at(1, 1); + double f = at(1, 2); + double g = at(2, 0); + double h = at(2, 1); + double i = at(2, 2); + return a * e * i + b * f * g + c * d * h - a * f * h - b * d * i + - c * e * g; + } + default: + { + int swapCount; + std::vector pivot; + Matrix LU = luDecompositionByGE(&swapCount, &pivot); + double result = 1.0; + for (std::size_t k = 0; k < LU.order(); k++) + { + result *= LU.at(pivot[k], k); + } + if (swapCount % 2 == 1) + { + result = -result; + } + return result; + } + } + } + + void Matrix::scaleRow(std::size_t r, double k) + { + for (std::size_t c = 0; c < m_cols; c++) + { + m_m[getIndex(r, c)] = k * at(r, c); + } + } + + void Matrix::combineRow(std::size_t srcRow, std::size_t dstRow, double k) + { + for (std::size_t c = 0; c < m_cols; c++) + { + m_m[getIndex(dstRow, c)] = at(dstRow, c) + k * at(srcRow, c); + } + } + + Matrix Matrix::luDecompositionByGE(int* p_swap_count, + std::vector* p_pivot) const + { + if (m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + std::vector& pivot = *p_pivot; + *p_swap_count = 0; + std::size_t n = rows(); + + // Make a copy of the matrix. + // We never change the original matrix because the backing array may be reused. + std::vector a(m_m); + Matrix m(n, n, a); + + initPivots(n, p_pivot); + for (std::size_t k = 0; k < n - 1; k++) + { + double maxValue = m.partialPivot(p_pivot, k, p_swap_count); + if (maxValue == 0) + { + throw std::runtime_error("Matrix is singular."); + } + double m_kk = m.at(pivot[k], k); + for (std::size_t i = k + 1; i < n; i++) + { + std::size_t ik = m.getIndex(pivot[i], k); + m.m_m[ik] /= m_kk; + } + for (std::size_t i = k + 1; i < n; i++) + { + double m_ik = m.at(pivot[i], k); + for (std::size_t j = k + 1; j < n; j++) + { + double m_kj = m.at(pivot[k], j); + std::size_t ij = m.getIndex(pivot[i], j); + m.m_m[ij] -= m_ik * m_kj; + } + } + } + if (m.at(pivot[n - 1], n - 1) == 0) + { + throw std::runtime_error("Matrix is singular."); + } + return m; + } + + Matrix Matrix::roundSymmetry() + { + std::vector a(m_m.size()); + for (std::size_t i = 0; i < m_rows; i++) + { + std::size_t diagIndex = getIndex(i, i); + a[diagIndex] = m_m[diagIndex]; + for (std::size_t j = i + 1; j < m_cols; j++) + { + std::size_t i1 = getIndex(i, j); + std::size_t i2 = getIndex(j, i); + double mean = (m_m[i1] + m_m[i2]) / 2; + a[i1] = a[i2] = mean; + } + } + return Matrix(m_rows, m_cols, a); + } + + std::size_t Matrix::getIndex(std::size_t row, std::size_t col) const + { + if (m_row_based) + { + return row * m_cols + col; + } + else + { + return col * m_rows + row; + } + } + + /* static */ + void Matrix::initPivots(std::size_t n, std::vector* p_pivot) + { + std::vector& pivot = *p_pivot; + pivot.resize(n); + for (std::size_t i = 0; i < pivot.size(); i++) + { + pivot[i] = i; + } + } + + double Matrix::partialPivot(std::vector* p_pivot, std::size_t k, int* p_swap_count) const + { + std::vector& pivot = *p_pivot; + int& swap_count = *p_swap_count; + double maxValue = fabs(at(pivot[k], k)); + std::size_t maxIndex = k; + for (std::size_t i = k + 1; i < pivot.size(); i++) + { + std::size_t rowIndex = pivot[i]; + double rowValue = fabs(at(rowIndex, k)); + if (rowValue > maxValue) + { + maxValue = rowValue; + maxIndex = i; + } + } + if (maxIndex != k) + { + std::size_t temp = pivot[maxIndex]; + pivot[maxIndex] = pivot[k]; + pivot[k] = temp; + swap_count++; + } + return maxValue; + } + + bool Matrix::isSymmetric() const + { + if (m_rows != m_cols) + { + return false; + } + for (std::size_t i = 1; i < rows(); i++) + { + for (std::size_t j = 1; j < i; j++) + { + if (at(i, j) != at(j, i)) + { + return false; + } + } + } + return true; + } + + Matrix Matrix::choleskyDecomposition() const + { + // returns an upper diagonal matrix + if (m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + if (!isSymmetric()) + { + throw std::runtime_error("Matrix must be symmetric."); + } + std::size_t n = rows(); + std::vector a(m_m); + for (std::size_t i = 0; i < n; i++) + { + std::size_t ii = getIndex(i, i); + for (std::size_t k = 0; k < i; k++) + { + std::size_t ki = getIndex(k, i); + a[ii] = a[ii] - a[ki] * a[ki]; + } + if (a[ii] < 0) + { + throw std::runtime_error("Matrix is not positive definite."); + } + a[ii] = std::sqrt(a[ii]); + for (std::size_t j = i + 1; j < n; j++) + { + std::size_t ij = getIndex(i, j); + for (std::size_t k = 0; k < i; k++) + { + std::size_t ki = getIndex(k, i); + std::size_t kj = getIndex(k, j); + a[ij] = a[ij] - a[ki] * a[kj]; + } + if (a[ij] != 0) { a[ij] = a[ij] / a[ii]; } + } + } + // Clear out the lower matrix + for (std::size_t i = 1; i < n; i++) + { + for (std::size_t j = 0; j < i; j++) + { + std::size_t ij = getIndex(i, j); + a[ij] = 0; + } + } + return Matrix(n, n, a); + } + + std::ostream& operator<<(std::ostream& os, const Matrix& m) + { + os << m.rows() << "x" << m.cols() << ":{"; + for (std::size_t row = 0; row < m.rows(); row++) + { + if (row > 0) { os << ", "; } + os << "{"; + for (std::size_t col = 0; col < m.cols(); col++) + { + if (col > 0) { os << ", "; } + os << m.at(row, col); + } + os << "}"; + } + os << "}"; + return os; + + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/src/probability/data/matrix_vector_operators.cpp b/core/navigation/localization/src/probability/data/matrix_vector_operators.cpp new file mode 100644 index 0000000..f1cc09f --- /dev/null +++ b/core/navigation/localization/src/probability/data/matrix_vector_operators.cpp @@ -0,0 +1,72 @@ +#include "localization/probability/data/matrix_vector_operators.hpp" +#include "localization/probability/data/matrix.hpp" +#include "localization/probability/data/vector.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + Vector operator*(const Matrix& m, const Vector& v) + { + if (m.cols() != v.order()) + { + std::stringstream error_message; + error_message << "Matrix and Vector sizes not compatible " + << "(" << m << " * " << v << ")"; + throw std::runtime_error(error_message.str()); + } + std::vector result(m.rows()); + for (std::size_t row = 0; row < m.rows(); row++) + { + double value = 0.0; + for (std::size_t col = 0; col < m.cols(); col++) + { + value += m.at(row, col) * v[col]; + } + result[row] = value; + } + return Vector(result); + } + + Vector operator*(const Vector& v, const Matrix& m) + { + if (v.order() != m.rows()) + { + throw std::runtime_error("Matrix size is not compatible with Vector size."); + } + std::vector result(v.order()); + for (std::size_t col = 0; col < m.cols(); col++) + { + for (std::size_t row = 0; row < m.rows(); row++) + { + result[row] += v[row] * m.at(row, col); + } + } + return Vector(result); + + } + + Matrix operator*(double scalar, const Matrix& m) + { + const std::vector& ma = m.m_m; + std::vector a(ma.size()); + for (std::size_t i = 0; i < a.size(); i++) + { + a[i] = ma[i] * scalar; + } + return Matrix(m.rows(), m.cols(), a); + + } + + double operator*(const Vector& v1, const Vector& v2) + { + return v1.dot(v2); + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/src/probability/data/vector.cpp b/core/navigation/localization/src/probability/data/vector.cpp new file mode 100644 index 0000000..c351eea --- /dev/null +++ b/core/navigation/localization/src/probability/data/vector.cpp @@ -0,0 +1,209 @@ +#include "localization/probability/data/vector.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + + Vector::Vector() + { + } + + Vector::Vector(std::vector a) : + m_a(std::move(a)) + { + } + + Vector::Vector(const Vector& v) : + m_a(v.m_a) + { + } + + Vector::Vector(Vector&& v) : + m_a(std::move(v.m_a)) + { + } + + Vector::Vector(std::size_t order) : + m_a(order) + { + } + + Vector::~Vector() + { + } + + double Vector::dot(const Vector& v2) const + { + const Vector& v1 = *this; + double result = 0.0; + for (std::size_t i = 0; i < v1.order(); i++) + { + result += v1.m_a[i] * v2.m_a[i]; + } + return result; + + } + + std::size_t Vector::order() const + { + return m_a.size(); + } + + void Vector::set(Vector a) + { + if (order() != a.order()) + { + throw std::runtime_error( + "Length of array is different from the Vector expected order."); + } + m_a = std::move(a.m_a); + } + + double& Vector::at(int i) + { + return m_a[i]; + } + + double Vector::at(int i) const + { + return m_a[i]; + } + + Vector Vector::operator+(const Vector& v2) const + { + const Vector& v1 = *this; + if (v1.order() != v2.order()) + { + throw std::runtime_error("Vector sizes are not compatible."); + } + Vector result(std::vector(v1.order())); + std::vector& a = result.m_a; + for (std::size_t i = 0; i < a.size(); i++) + { + a[i] = v1.m_a[i] + v2.m_a[i]; + } + return result; + } + + Vector Vector::operator-(const Vector& v2) const + { + const Vector& v1 = *this; + if (v1.order() != v2.order()) + { + throw std::runtime_error("Vector sizes are not compatible."); + } + Vector result(v1.order()); + std::vector& a = result.m_a; + for (std::size_t i = 0; i < a.size(); i++) + { + a[i] = v1.m_a[i] - v2.m_a[i]; + } + return result; + } + + bool Vector::operator<(const Vector& v2) const + { + if (this == &v2) { return false; } + const Vector& v1 = *this; + bool result = v1.order() > 0; + for (std::size_t i = 0; i < v1.order(); i++) + { + result = result && v1.m_a[i] < v2.m_a[i]; + } + return result; + } + + bool Vector::operator>(const Vector& v2) const + { + if (this == &v2) { return false; } + const Vector& v1 = *this; + bool result = v1.order() > 0; + for (std::size_t i = 0; i < v1.order(); i++) + { + result = result && v1.m_a[i] > v2.m_a[i]; + } + return result; + } + + bool Vector::operator==(const Vector& v) const + { + if (this == &v) { return true; } + return m_a == v.m_a; + } + + Vector& Vector::operator=(const Vector& that) + { + this->m_a = that.m_a; + return *this; + } + +// operator= is implicitly declared as deleted in subclasses +// because it also defines a move assignment operator. + void Vector::copyAssign(const Vector& v) + { + if (this != &v) + { + m_a = v.m_a; + } + } + + void Vector::moveAssign(Vector&& v) + { + if (this != &v) + { + m_a = std::move(v.m_a); + } + } + + double& Vector::operator[](std::size_t i) + { + return m_a[i]; + } + + double Vector::operator[](std::size_t i) const + { + return m_a[i]; + } + + Matrix Vector::cross(const Vector& that) const + { + std::vector m(order() * that.order()); + int k = 0; + for (std::size_t i = 0; i < order(); i++) + { + double a = m_a[i]; + for (std::size_t j = 0; j < that.order(); j++) + { + double b = that[j]; + m[k] = a * b; + k++; + } + } + return Matrix(order(), that.order(), m); + } + + Vector Vector::aliasVector() const + { + return Vector(m_a); + } + + std::ostream& operator<<(std::ostream& os, const Vector& v) + { + os << v.order() << ":{"; + for (std::size_t i = 0; i < v.order(); i++) + { + if (i > 0) { os << ", "; } + os << v[i]; + } + os << "}"; + return os; + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/test/main.cpp b/core/navigation/localization/test/main.cpp new file mode 100644 index 0000000..ce0f6f1 --- /dev/null +++ b/core/navigation/localization/test/main.cpp @@ -0,0 +1,8 @@ + +#include "gtest/gtest.h" + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/core/navigation/localization/test/test_bayes_distribution.cpp b/core/navigation/localization/test/test_bayes_distribution.cpp new file mode 100644 index 0000000..a37cc21 --- /dev/null +++ b/core/navigation/localization/test/test_bayes_distribution.cpp @@ -0,0 +1,111 @@ +#include + +#include "localization/probability/random_conditional.hpp" +#include "localization/probability/ranged_uniform.hpp" +#include "localization/probability/bayes_distribution.hpp" +#include "localization/sensor_model.hpp" + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + namespace test + { + DEFINE_VECTOR1(X); + DEFINE_VECTOR1(Z); + + /** + * A sensor model that says that X is within 0.25 units observed Z + */ + class SensorModel : public RandomConditional + { + public: + double conditionalProbabilityOf(const Z& z, const X& x) const override + { + if(fabs(z[0] - x[0]) < 0.25) + { + return 2.0; + } + return 0.0; + } + }; + + typedef BayesDistribution BayesXZ; + + TEST(BayesDistributionTest, probabilityOf) + { + auto belief = std::make_shared>(X({0}), X({2})); + auto sensor_model = std::make_shared(); + auto bd = std::make_shared(belief, sensor_model, 0.5); + // Probabilities are not normalized. They should be treated proportionally + // and not as exact probabilities. + EXPECT_EQ(0.0, bd->probabilityOf(0.1)); + EXPECT_EQ(1.0, bd->probabilityOf(0.3)); + EXPECT_EQ(1.0, bd->probabilityOf(0.5)); + EXPECT_EQ(1.0, bd->probabilityOf(0.7)); + EXPECT_EQ(0.0, bd->probabilityOf(0.9)); + } + + TEST(BayesDistributionTest, sample) + { + std::default_random_engine generator(0); + auto belief = std::make_shared< RangedUniform >(X({0}), X({2})); + auto sensor_model = std::make_shared(); + auto bd = std::make_shared(belief, sensor_model, 0.5); + // Sample is not implemented in Bayes Distribution + ASSERT_THROW(bd->sample(&generator), std::runtime_error); + } + + TEST(BayesDistributionTest, chainObservation) + { + auto belief = std::make_shared>(X({0}), X({2})); + auto sensor_model = std::make_shared(); + auto bd = std::make_shared(belief, sensor_model, 0.5); + bd = bd->chainObservation(0.9); + // Probabilities are not normalized. They should be treated proportionally + // and not as exact probabilities. + EXPECT_EQ(0.0, bd->probabilityOf(0.1)); + EXPECT_EQ(0.0, bd->probabilityOf(0.3)); + EXPECT_EQ(0.0, bd->probabilityOf(0.5)); + EXPECT_EQ(2.0, bd->probabilityOf(0.7)); + EXPECT_EQ(0.0, bd->probabilityOf(0.9)); + } + + TEST(BayesDistributionTest, estimateNormalizer) + { + auto belief = std::make_shared< RangedUniform >(X({0}), X({2})); + auto sensor_model = std::make_shared(); + auto bd = std::make_shared(belief, sensor_model, 0.5); + std::vector samples; + double dx = 0.02; + samples.reserve((long)(4 / dx)); + for(double x = -1.0; x < 3.0; x += dx) + { + samples.push_back(x); + } + EXPECT_EQ(0.5, bd->estimateNormalizer(samples, 0.02)); + EXPECT_EQ(0.5, bd->normalizer()); + EXPECT_EQ(0.0, bd->probabilityOf(0.1)); + EXPECT_EQ(2.0, bd->probabilityOf(0.3)); + EXPECT_EQ(2.0, bd->probabilityOf(0.5)); + EXPECT_EQ(2.0, bd->probabilityOf(0.7)); + EXPECT_EQ(0.0, bd->probabilityOf(0.9)); + } + + TEST(BayesDistributionTest, properties) + { + auto belief = std::make_shared>(X({0}), X({2})); + auto sensor_model = std::make_shared(); + auto bd = std::make_shared(belief, sensor_model, 0.5); + EXPECT_EQ(sensor_model.get(), &bd->sensorModel()); + EXPECT_EQ(Z(0.5), bd->data()); + } + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/test/test_conditional_map.hpp.cpp b/core/navigation/localization/test/test_conditional_map.hpp.cpp new file mode 100644 index 0000000..5fd6cd4 --- /dev/null +++ b/core/navigation/localization/test/test_conditional_map.hpp.cpp @@ -0,0 +1,38 @@ +#include +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + namespace test + { + DEFINE_VECTOR1(X); + enum Y + { + y0, + y1 + }; + + TEST(ConditionalMapTest, GetSet) { + ConditionalMap px_given_y; + px_given_y.set(y0, std::make_shared>(X(0.5), X(1.0))); + px_given_y.set(y1, std::make_shared>(X(0.0), X(2.0))); + EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(0.4, y0)); + EXPECT_EQ(2.0, px_given_y.conditionalProbabilityOf(0.8, y0)); + EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(1.5, y0)); + EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(-1.0, y1)); + EXPECT_EQ(0.5, px_given_y.conditionalProbabilityOf(1.5, y1)); + EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(3.0, y1)); + } + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/test/test_conditional_value_map.cpp b/core/navigation/localization/test/test_conditional_value_map.cpp new file mode 100644 index 0000000..5c1ec45 --- /dev/null +++ b/core/navigation/localization/test/test_conditional_value_map.cpp @@ -0,0 +1,132 @@ +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + namespace test + { + enum class X + { + x0, x1 + }; + + enum class Y + { + y0, y1 + }; + + enum class Z + { + z0, z1 + }; + + typedef ConditionalValueMap CvmXY; + typedef ConditionalValueMap CvmYZ; + + TEST(ConditionalValueMapTest, DomainRange) + { + CvmXY xy; + xy.set(X::x0, Y::y0, 1.0); + xy.set(X::x1, Y::y0, 1.0); + EXPECT_EQ(std::vector({X::x0, X::x1}), xy.domain()); + EXPECT_EQ(std::vector({Y::y0}), xy.range()); + } + + TEST(ConditionalValueMapTest, SetGet) + { + CvmXY xy; + xy.set(X::x0, Y::y0, 0.9); + xy.set(X::x1, Y::y0, 0.1); + xy.set(X::x0, Y::y1, 0.2); + xy.set(X::x1, Y::y1, 0.8); + EXPECT_EQ(0.9, xy.conditionalProbabilityOf(X::x0, Y::y0)); + EXPECT_EQ(0.1, xy.conditionalProbabilityOf(X::x1, Y::y0)); + EXPECT_EQ(0.2, xy.conditionalProbabilityOf(X::x0, Y::y1)); + EXPECT_EQ(0.8, xy.conditionalProbabilityOf(X::x1, Y::y1)); + } + + TEST(ConditionalValueMapTest, LikelihoodOf) + { + CvmXY xy; + xy.set(X::x0, Y::y0, 0.9); + xy.set(X::x1, Y::y0, 0.1); + xy.set(X::x0, Y::y1, 0.2); + xy.set(X::x1, Y::y1, 0.8); + + auto lh0 = xy.likelihoodOf(X::x0); + EXPECT_DOUBLE_EQ(0.9, lh0->probabilityOf(Y::y0)); + EXPECT_DOUBLE_EQ(0.2, lh0->probabilityOf(Y::y1)); + lh0->normalize(); + EXPECT_DOUBLE_EQ(9.0 / 11.0, lh0->probabilityOf(Y::y0)); + EXPECT_DOUBLE_EQ(2.0 / 11.0, lh0->probabilityOf(Y::y1)); + + auto lh1 = xy.likelihoodOf(X::x1); + EXPECT_DOUBLE_EQ(0.1, lh1->probabilityOf(Y::y0)); + EXPECT_DOUBLE_EQ(0.8, lh1->probabilityOf(Y::y1)); + lh1->normalize(); + EXPECT_DOUBLE_EQ(1.0 / 9.0, lh1->probabilityOf(Y::y0)); + EXPECT_DOUBLE_EQ(8.0 / 9.0, lh1->probabilityOf(Y::y1)); + } + + TEST(ConditionalValueMapTest, BayesianInference) + { + CvmXY xy; + xy.set(X::x0, Y::y0, 0.9); + xy.set(X::x1, Y::y0, 0.1); + xy.set(X::x0, Y::y1, 0.2); + xy.set(X::x1, Y::y1, 0.8); + DistributionValueMap belief0(Y::y0, 0.1, Y::y1, 0.9); + auto belief1 = xy.bayesianInference(X::x0, belief0); + EXPECT_DOUBLE_EQ(9.0 / 27.0, belief1->probabilityOf(Y::y0)); + EXPECT_DOUBLE_EQ(18.0 / 27.0, belief1->probabilityOf(Y::y1)); + } + + TEST(ConditionalValueMapTest, Marginalize) + { + CvmXY xy; + xy.set(X::x0, Y::y0, 0.9); + xy.set(X::x1, Y::y0, 0.1); + xy.set(X::x0, Y::y1, 0.2); + xy.set(X::x1, Y::y1, 0.8); + + CvmYZ yz; + yz.set(Y::y0, Z::z0, 0.9); + yz.set(Y::y1, Z::z0, 0.1); + yz.set(Y::y0, Z::z1, 0.2); + yz.set(Y::y1, Z::z1, 0.8); + + auto xz = xy.marginalize(yz); + EXPECT_DOUBLE_EQ(0.83, xz->conditionalProbabilityOf(X::x0, Z::z0)); + EXPECT_DOUBLE_EQ(0.17, xz->conditionalProbabilityOf(X::x1, Z::z0)); + EXPECT_DOUBLE_EQ(0.34, xz->conditionalProbabilityOf(X::x0, Z::z1)); + EXPECT_DOUBLE_EQ(0.66, xz->conditionalProbabilityOf(X::x1, Z::z1)); + } + + TEST(ConditionalValueMapTest, SampleLikelihood) + { + CvmXY xy; + xy.set(X::x0, Y::y0, 0.9); + xy.set(X::x1, Y::y0, 0.1); + xy.set(X::x0, Y::y1, 0.2); + xy.set(X::x1, Y::y1, 0.8); + + EXPECT_EQ(1.1, xy.sumLikelihood(X::x0)); + EXPECT_EQ(0.9, xy.sumLikelihood(X::x1)); + + EXPECT_EQ(Y::y0, xy.sampleLikelihood(X::x0, 0.85)); + EXPECT_EQ(Y::y1, xy.sampleLikelihood(X::x0, 0.95)); + EXPECT_EQ(Y::y0, xy.sampleLikelihood(X::x1, 0.05)); + EXPECT_EQ(Y::y1, xy.sampleLikelihood(X::x1, 0.15)); + } + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/test/test_independent_pair_distribution.cpp b/core/navigation/localization/test/test_independent_pair_distribution.cpp new file mode 100644 index 0000000..6d074f5 --- /dev/null +++ b/core/navigation/localization/test/test_independent_pair_distribution.cpp @@ -0,0 +1,75 @@ +#include + +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + namespace test + { + enum X { x0, x1 }; + enum Y { y0, y1 }; + typedef std::pair XY; + + TEST(IndependentPairDistributionTest, ProbabilityOf) + { + auto x = std::make_shared>(x0, 0.1, x1, 0.9); + auto y = std::make_shared>(y0, 0.2, y1, 0.8); + auto xy = std::make_shared>(x, y); + EXPECT_DOUBLE_EQ(0.02, xy->probabilityOf(XY(x0, y0))); + EXPECT_DOUBLE_EQ(0.08, xy->probabilityOf(XY(x0, y1))); + EXPECT_DOUBLE_EQ(0.18, xy->probabilityOf(XY(x1, y0))); + EXPECT_DOUBLE_EQ(0.72, xy->probabilityOf(XY(x1, y1))); + } + + TEST(IndependentPairDistributionTest, Sample) + { + std::default_random_engine generator(0); + auto x = std::make_shared>(x0, 0.1, x1, 0.9); + auto y = std::make_shared>(y0, 0.2, y1, 0.8); + auto xy = std::make_shared>(x, y); + double x0y0_count = 0, x1y0_count = 0, x0y1_count = 0, x1y1_count = 0; + int n = 1000; + for (int i = 0; i < n; ++i) + { + auto sample = xy->sample(&generator); + if (sample == XY(x0, y0)) + { + x0y0_count++; + } + + if (sample == XY(x0, y1)) + { + x0y1_count++; + } + if (sample == XY(x1, y0)) + { + x1y0_count++; + } + if (sample == XY(x1, y1)) + { + x1y1_count++; + } + } + std::cout << "Selection counts: " + << x0y0_count << " " + << x0y1_count << " " + << x1y0_count << " " + << x1y1_count << std::endl; + EXPECT_TRUE(x0y0_count > 10 && x0y0_count < 30); + EXPECT_TRUE(x0y1_count > 40 && x0y0_count < 120); + EXPECT_TRUE(x1y0_count > 90 && x0y0_count < 270); + EXPECT_TRUE(x1y1_count > 620 && x0y0_count < 820); + } + + } + } + } + } +} \ No newline at end of file diff --git a/core/navigation/localization/test/test_random_distribution.cpp b/core/navigation/localization/test/test_random_distribution.cpp new file mode 100644 index 0000000..e5f8f52 --- /dev/null +++ b/core/navigation/localization/test/test_random_distribution.cpp @@ -0,0 +1,46 @@ +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + namespace test + { + + DEFINE_VECTOR1(X); + + class Impulse : RandomDistribution + { + public: + double probabilityOf(const X& x) const override + { + if(x[0] == 0.0) + return 1.0; + return 0.0; + } + + X sample(std::default_random_engine*) const override + { + return 0.0; + } + }; + + TEST(RandomDistributionTest, ctor1) + { + std::default_random_engine generator(0); + Impulse d; + ASSERT_EQ(1.0, d.probabilityOf(0.0)); + ASSERT_EQ(0.0, d.probabilityOf(0.5)); + ASSERT_EQ(X(0.0), d.sample(&generator)); + } + } + } + } + } +} diff --git a/core/navigation/localization/test/test_ranged_uniform.cpp b/core/navigation/localization/test/test_ranged_uniform.cpp new file mode 100644 index 0000000..6629154 --- /dev/null +++ b/core/navigation/localization/test/test_ranged_uniform.cpp @@ -0,0 +1,44 @@ +#include +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace localization + { + namespace probability + { + namespace test + { + DEFINE_VECTOR1(X); + + TEST(RangedUniformTest, ProbabilityOf) + { + auto x = std::make_shared> + (X({0.25}), X({0.75})); + EXPECT_EQ(0.0, x->probabilityOf(0.1)); + EXPECT_EQ(2.0, x->probabilityOf(0.3)); + EXPECT_EQ(2.0, x->probabilityOf(0.5)); + EXPECT_EQ(2.0, x->probabilityOf(0.7)); + EXPECT_EQ(0.0, x->probabilityOf(0.9)); + } + + TEST(RangedUniformTest, Sample) + { + std::default_random_engine generator(0); + auto x = std::make_shared> + (X({0.25}), X({0.75})); + for(int i = 0; i < 1000; i ++) + { + X sample = x->sample(&generator); + EXPECT_TRUE(sample[0] < 0.75 && sample[0] > 0.25); + } + } + } + } + } + } +} From cd4ae3141fd3a2832e7088474f23aa5df5b559c1 Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Tue, 20 Mar 2018 17:29:29 -0500 Subject: [PATCH 048/200] Added Roboclaw Header and Source --- CMakeLists.txt | 6 +- appendages/include/appendages/exceptions.hpp | 7 +- appendages/include/appendages/roboclaw.hpp | 67 ++++++++++++++++++++ appendages/src/appendage_factory.cpp | 2 + appendages/src/roboclaw.cpp | 67 ++++++++++++++++++++ 5 files changed, 145 insertions(+), 4 deletions(-) create mode 100644 appendages/include/appendages/roboclaw.hpp create mode 100644 appendages/src/roboclaw.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5446560..fcea426 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,11 @@ endif() # to correctly compile the codebase despite this line being in the # downstream CMake files. set (CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") + +option(GCC_DIAG_COLORS "Enable GCC Diagnostic Colors" ON) +if (GCC_DIAG_COLORS) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") +endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) diff --git a/appendages/include/appendages/exceptions.hpp b/appendages/include/appendages/exceptions.hpp index c7ddb44..56576f7 100644 --- a/appendages/include/appendages/exceptions.hpp +++ b/appendages/include/appendages/exceptions.hpp @@ -17,8 +17,8 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#ifndef EXCEPTIONS_HPP -#define EXCEPTIONS_HPP +#ifndef APPENDAGES_EXCEPTIONS_HPP +#define APPENDAGES_EXCEPTIONS_HPP #include namespace rip @@ -29,6 +29,7 @@ namespace rip NEW_EX(AppendageWithoutType) NEW_EX(AppendageWithoutLabel) NEW_EX(AppendageWithId) + NEW_EX(AppendageMissingField) } } -#endif // EXCEPTIONS_HPP +#endif // APPENDAGES_EXCEPTIONS_HPP diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp new file mode 100644 index 0000000..c650750 --- /dev/null +++ b/appendages/include/appendages/roboclaw.hpp @@ -0,0 +1,67 @@ +#ifndef ROBOCLAW_HPP +#define ROBOCLAW_HPP + +#include + +#include + +#include "appendages/appendage.hpp" +#include "appendages/appendage_factory.hpp" + +#include + +namespace rip +{ + namespace appendages + { + class Roboclaw : public Appendage + { + public: + void SetSpeed(int32_t speed1, int32_t speed2); + void SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); + std::tuple ReadEncoders(); + void SetDuty(int16_t duty1, int16_t duty2); + void SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps); + + /** + * Stop + */ + virtual void stop() override; + + virtual bool diagnostic() override; + + protected: + friend class AppendageFactory; + + /** + * Function wrapper for the constructor so it can be pointed to + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + static std::shared_ptr create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + private: + /** + * Constructor + * + * @param config The config from arduino gen + * @param command_map A map of the name of the commands to their enumerations + * @param device The connection to the device + */ + Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); + + uint8_t m_address; + + std::shared_ptr m_set_speed; + std::shared_ptr m_set_speed_accel; + std::shared_ptr m_read_encoders; + std::shared_ptr m_read_encoders_result; + std::shared_ptr m_set_duty; + std::shared_ptr m_set_velocity_pid; + }; + } +} + +#endif // ROBOCLAW_HPP \ No newline at end of file diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index ae785d3..1034978 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -5,6 +5,7 @@ // Appendages #include "appendages/digital_input.hpp" #include "appendages/analog_input.hpp" +#include "appendages/roboclaw.hpp" #include "appendages/exceptions.hpp" @@ -45,6 +46,7 @@ namespace rip { registerAppendage("Digital Input", &DigitalInput::create); registerAppendage("Analog Input", &AnalogInput::create); + registerAppendage("Roboclaw", &Roboclaw::create); } } } diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp new file mode 100644 index 0000000..aa5dfe7 --- /dev/null +++ b/appendages/src/roboclaw.cpp @@ -0,0 +1,67 @@ +#include "appendages/roboclaw.hpp" + +#include +#include +#include + +#include + +#include "appendages/exceptions.hpp" + +namespace rip +{ + namespace appendages + { + Roboclaw::Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + : Appendage(config, device), + m_set_speed(createCommand("kSetSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoders(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_duty(createCommand("kSetDuty", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + { + if (config.find("address") == config.end()) + { + throw AppendageMissingField(fmt::format("appendage roboclaw missing address")); + } + m_address = config["address"]; + } + + std::shared_ptr Roboclaw::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) + { + return std::dynamic_pointer_cast(std::shared_ptr(new Roboclaw(config, command_map, device))); + } + + void Roboclaw::SetSpeed(int32_t speed1, int32_t speed2) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_speed, m_address, speed1, speed2); + } + + void Roboclaw::SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + } + + std::tuple Roboclaw::ReadEncoders() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_encoders, m_address); + return messenger.receive(m_read_encoders_result); + } + + void Roboclaw::SetDuty(int16_t duty1, int16_t duty2) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_duty, m_address, duty1, duty2); + } + + void Roboclaw::SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_velocity_pid, m_address, motor, Kp, Ki, Kd, qpps); + } + } +} \ No newline at end of file From 304470f78d45a22e905aa6988fa89b34d226070f Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 20 Mar 2018 18:48:34 -0400 Subject: [PATCH 049/200] Enable build_rip_rpi --- go-docker.sh | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/go-docker.sh b/go-docker.sh index c1f8ff9..9f1a007 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -91,6 +91,22 @@ case $RIPPROG in echo "Building rip_deps container..." DOCKER_VTAG=latest ./docker/rip_deps.sh ;; + build_rip_rpi) + if ($PROMPTER --title "Comfirm Build?" --yesno "Do not run this command unless you were told to do so. Requires files outside of RIP." 8 68) then + if find ../rpi_images -prune -empty ; then + rpi_image_file="$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | tail -1 )" + echo "Found rpi debootstrap image ${rpi_image_file}" + rsync --copy-links --times ../rpi_images/${rpi_image_file} ./external/rpi_images/ + echo "Building the rip_rpi container... this will take awhile." + docker build -f docker/rip_rpi.dockerfile -t utkrobotics/rip_rpi:latest --build-arg pi_image=${rpi_image_file} . + else + echo "No Raspbian debootstrap images could be located at ../rpi_images." + exit 1 + fi + else + echo "Run me again to select another option." + fi + ;; cancel) echo "I'm a very busy shell script, next time try not to waste my time? Thanks!" ;; From 95738e59fe73720e21e4f55bc135dde41839255b Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 16:00:21 -0400 Subject: [PATCH 050/200] Fix incomplete vtable in Roboclaw appendage --- appendages/src/roboclaw.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index aa5dfe7..caa355d 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -63,5 +63,13 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_velocity_pid, m_address, motor, Kp, Ki, Kd, qpps); } + + void Roboclaw::stop() { + SetSpeed(0, 0); + } + + bool Roboclaw::diagnostic() { + // TODO + } } -} \ No newline at end of file +} From 26a118ac114ab8e87b7f7d1052fe9b3930a9f985 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 17:14:35 -0400 Subject: [PATCH 051/200] Add platformio to rip_deps for arduinogen --- docker/rip_deps.dockerfile | 6 ++++++ docker/rip_deps.sh | 22 ++++++++++++++-------- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/docker/rip_deps.dockerfile b/docker/rip_deps.dockerfile index 2ea84b2..bb193bf 100644 --- a/docker/rip_deps.dockerfile +++ b/docker/rip_deps.dockerfile @@ -29,4 +29,10 @@ RUN cmake .. RUN make -j$(nproc --ignore=1 ) # building g2o might take awhile... RUN sudo make install +RUN sudo install_clean \ + python-pip python-setuptools + +RUN sudo pip install --no-cache-dir --upgrade pip +RUN sudo pip install --no-cache-dir platformio + # vim: set syntax=dockerfile: diff --git a/docker/rip_deps.sh b/docker/rip_deps.sh index fd70029..4b17d9a 100755 --- a/docker/rip_deps.sh +++ b/docker/rip_deps.sh @@ -8,15 +8,21 @@ source docker/core.sh DOCKER_VTAG=${DOCKER_VTAG:-$(git_branch_norm)} prompt_docker_vtag -stdbuf -o0 \ -docker build \ +if [[ "$DOCKER_STDOUT" == "raw" ]]; then + docker build \ -f docker/rip_deps.dockerfile \ - --tag utkrobotics/rip_deps:${DOCKER_VTAG} . | \ -stdbuf -o0 grep -e '^Step' | \ -stdbuf -o0 cut -d ' ' -f2,4- | \ -stdbuf -o0 sed 's;/; ;1' | \ -stdbuf -o0 awk '{pc=100*($1/$2);i=int(pc);print "XXX\n" i "\n" $0 "\nXXX"}' | \ -$PROMPTER --title "Building rip_deps:${DOCKER_VTAG} container" --gauge "Starting Docker Build..." $(( LINES - 4 )) $(( COLUMNS - 18 )) $(( $LINES - 12 )) + --tag utkrobotics/rip_deps:${DOCKER_VTAG} . +else + stdbuf -o0 \ + docker build \ + -f docker/rip_deps.dockerfile \ + --tag utkrobotics/rip_deps:${DOCKER_VTAG} . | \ + stdbuf -o0 grep -e '^Step' | \ + stdbuf -o0 cut -d ' ' -f2,4- | \ + stdbuf -o0 sed 's;/; ;1' | \ + stdbuf -o0 awk '{pc=100*($1/$2);i=int(pc);print "XXX\n" i "\n" $0 "\nXXX"}' | \ + $PROMPTER --title "Building rip_deps:${DOCKER_VTAG} container" --gauge "Starting Docker Build..." $(( LINES - 4 )) $(( COLUMNS - 18 )) $(( $LINES - 12 )) +fi popd popd From d2941772bd6c757959cf1eb62bc024dfcbdeab47 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 17:55:43 -0400 Subject: [PATCH 052/200] Refactor go-docker +build_rip_rpi --- go-docker.sh | 86 +++++++++++++++++++++++++++++++++++----------------- 1 file changed, 58 insertions(+), 28 deletions(-) diff --git a/go-docker.sh b/go-docker.sh index 9f1a007..273170d 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -14,6 +14,14 @@ while [[ "$1" != "" ]] ; do --rpi) RIPPROG=run_rpi ;; + --prog) + if [[ "$1" != "" ]]; then + RIPPROG="$1" + else + echo " --prog requires a program selection to run." + exit 1 + fi + break --) shift if [[ "$1" != "" ]]; then @@ -38,27 +46,60 @@ RIPPROG=$($PROMPTER --title "Choose Action" \ "build_rip_deps" "Build the rip_deps base dependency image." \ "update_rpi" "Pull latest rip_rpi image from the dockerhub." \ "build_rip_rpi" "Build the rip_rpi Pi emulator and dependency container." \ + "build_stack" "Build the full RIP docker stack." \ "cancel" "Do nothing." \ 3>&1 1>&2 2>&3 ) fi +function build_rip_deps() { + echo "Building rip_deps container..." + DOCKER_VTAG=latest ./docker/rip_deps.sh +} + +function build_rip_rpi() { + if ($PROMPTER --title "Comfirm Build?" --yesno "Do not run this command unless you were told to do so. Requires files outside of RIP." 8 68) then + if find ../rpi_images -prune -empty ; then + rpi_image_file="$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | tail -1 )" + echo "Found rpi debootstrap image ${rpi_image_file}" + rsync --copy-links --times ../rpi_images/${rpi_image_file} ./external/rpi_images/ + echo "Building the rip_rpi container... this will take awhile." + docker build -f docker/rip_rpi.dockerfile -t utkrobotics/rip_rpi:latest --build-arg pi_image=${rpi_image_file} . + else + echo "No Raspbian debootstrap images could be located at ../rpi_images." + exit 1 + fi + else + echo "Run me again to select another option." + fi +} + +function build_stack() { + docker pull robobenklein/home + build_rip_deps + build_rip_rpi +} + +function run_rpi() { + # go to the user's working dir and run the container there. + ABSPATH_RPXC="$(readlink -f $RPXC_SCRIPT_LOCATION )" + popd + "${ABSPATH_RPXC}" --image "$RPXC_IMAGE" \ + --args "--rm \ + --tmpfs=${RPXC_SYSROOT}/dev:rw,dev \ + --mount type=tmpfs,dst=${RPXC_SYSROOT}/dev/shm \ + --mount type=bind,src=/dev/pts,dst=/dev/pts \ + --mount type=bind,src=/dev/pts,dst=${RPXC_SYSROOT}/dev/pts \ + " -- ${DEF_EXEC} + pushd "$SELFDIR" +} + if [ -n "$RIPPROG" ]; then - echo "Container command: ‘${(j: Ø :)DEF_EXEC}’" +echo "Container command: ‘${(j: Ø :)DEF_EXEC}’" echo "( $0 -- to change it )" case $RIPPROG in run_rpi) - # go to the user's working dir and run the container there. - ABSPATH_RPXC="$(readlink -f $RPXC_SCRIPT_LOCATION )" - popd - "${ABSPATH_RPXC}" --image "$RPXC_IMAGE" \ - --args "--rm \ - --tmpfs=${RPXC_SYSROOT}/dev:rw,dev \ - --mount type=tmpfs,dst=${RPXC_SYSROOT}/dev/shm \ - --mount type=bind,src=/dev/pts,dst=/dev/pts \ - --mount type=bind,src=/dev/pts,dst=${RPXC_SYSROOT}/dev/pts \ - " -- ${DEF_EXEC} - pushd "$SELFDIR" + run_rpi ;; new_rip) DOCKER_VTAG=${DOCKER_VTAG:-$(git_branch_norm)} @@ -88,24 +129,13 @@ case $RIPPROG in docker pull $RPXC_IMAGE ;; build_rip_deps) - echo "Building rip_deps container..." - DOCKER_VTAG=latest ./docker/rip_deps.sh + build_rip_deps ;; build_rip_rpi) - if ($PROMPTER --title "Comfirm Build?" --yesno "Do not run this command unless you were told to do so. Requires files outside of RIP." 8 68) then - if find ../rpi_images -prune -empty ; then - rpi_image_file="$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | tail -1 )" - echo "Found rpi debootstrap image ${rpi_image_file}" - rsync --copy-links --times ../rpi_images/${rpi_image_file} ./external/rpi_images/ - echo "Building the rip_rpi container... this will take awhile." - docker build -f docker/rip_rpi.dockerfile -t utkrobotics/rip_rpi:latest --build-arg pi_image=${rpi_image_file} . - else - echo "No Raspbian debootstrap images could be located at ../rpi_images." - exit 1 - fi - else - echo "Run me again to select another option." - fi + build_rip_rpi + ;; + build_stack) + build_stack ;; cancel) echo "I'm a very busy shell script, next time try not to waste my time? Thanks!" From c619d0d2c91c06d18c8512b6b8ea3cbbdc81de27 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 18:03:11 -0400 Subject: [PATCH 053/200] Fix open case --- go-docker.sh | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/go-docker.sh b/go-docker.sh index 273170d..d690a71 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -15,14 +15,15 @@ while [[ "$1" != "" ]] ; do RIPPROG=run_rpi ;; --prog) + shift if [[ "$1" != "" ]]; then RIPPROG="$1" else echo " --prog requires a program selection to run." exit 1 fi - break - --) + ;; + '--') shift if [[ "$1" != "" ]]; then DEF_EXEC=("$@") From 1b7f380aadf091644b98d9fb492be821b0a77ead Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Sun, 25 Mar 2018 17:24:26 -0500 Subject: [PATCH 054/200] Added check if code_template.txt exists. --- arduino_gen/include/arduino_gen/exceptions.hpp | 6 +++--- arduino_gen/src/arduino_gen.cpp | 8 +++++++- arduino_gen/src/main.cpp | 5 +++-- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/arduino_gen/include/arduino_gen/exceptions.hpp b/arduino_gen/include/arduino_gen/exceptions.hpp index 3a1c97e..6642034 100644 --- a/arduino_gen/include/arduino_gen/exceptions.hpp +++ b/arduino_gen/include/arduino_gen/exceptions.hpp @@ -1,5 +1,5 @@ -#ifndef EXCEPTIONS_HPP -#define EXCEPTIONS_HPP +#ifndef ARDUINO_GEN_EXCEPTIONS_HPP +#define ARDUINO_GEN_EXCEPTIONS_HPP #include #include @@ -45,4 +45,4 @@ namespace rip } } -#endif // EXCEPTIONS_HPP +#endif // ARDUINO_GEN_EXCEPTIONS_HPP diff --git a/arduino_gen/src/arduino_gen.cpp b/arduino_gen/src/arduino_gen.cpp index 8051a65..ae2313e 100644 --- a/arduino_gen/src/arduino_gen.cpp +++ b/arduino_gen/src/arduino_gen.cpp @@ -174,7 +174,13 @@ namespace rip std::string ArduinoGen::getArduinoCode() { - std::unique_ptr code_template_istream = cppfs::fs::open("code_template.txt").createInputStream(); + cppfs::FileHandle code_template_fh = cppfs::fs::open("code_template.txt"); + if (!code_template_fh.exists() || !code_template_fh.isFile()) + { + throw FileIoException("code_template.txt does not exist"); + } + + std::unique_ptr code_template_istream = code_template_fh.createInputStream(); std::string code_template(std::istreambuf_iterator(*code_template_istream), {}); return fmt::format(code_template, diff --git a/arduino_gen/src/main.cpp b/arduino_gen/src/main.cpp index 69920f6..fb9bcf7 100644 --- a/arduino_gen/src/main.cpp +++ b/arduino_gen/src/main.cpp @@ -7,6 +7,7 @@ #include #include "arduino_gen/arduino_gen.hpp" +#include "misc/exception_base.hpp" int main(int argc, char* argv[]) { @@ -96,7 +97,7 @@ int main(int argc, char* argv[]) { ag.readConfig(args::get(config)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to read the config file.\n" << e.what() << std::endl; return EXIT_FAILURE; @@ -106,7 +107,7 @@ int main(int argc, char* argv[]) { ag.generateOutput(!args::get(noCopy)); } - catch (std::exception e) + catch (rip::utilities::ExceptionBase e) { std::cerr << "ArduinoGen failed to generate the output.\n" << e.what() << std::endl; return EXIT_FAILURE; From c908db760d505e1336195f97f08890f8a6f5ff16 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 18:42:38 -0400 Subject: [PATCH 055/200] Build separate dirs for armhf --- .gitignore | 1 + build-linux.sh | 18 ++++++++++++------ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index 562ed72..f9b09a9 100644 --- a/.gitignore +++ b/.gitignore @@ -88,6 +88,7 @@ Thumbs.db *.pyc build/ +build_armhf/ doc/ .idea/ cmake-build-debug/ diff --git a/build-linux.sh b/build-linux.sh index 51bda5a..1f439ce 100755 --- a/build-linux.sh +++ b/build-linux.sh @@ -1,6 +1,12 @@ #!/bin/zsh set -E +BUILDDIR="build" + +if [[ "$(dpkg --print-architecture)" == "armhf" ]]; then + BUILDDIR="${BUILDDIR}_armhf" +fi + while [[ "$1" != "" ]]; do case "$1" in "--help"|"-h") @@ -14,11 +20,11 @@ while [[ "$1" != "" ]]; do break ;; "--clean"|"-c") - if [ -d build ]; then - echo "Removing $(pwd)/build/" - rm -r "$(pwd)/build/" + if [ -d "$BUILDDIR" ]; then + echo "Removing $(pwd)/${BUILDDIR}/" + rm -r "$(pwd)/${BUILDDIR}/" else - echo "No $(pwd)/build/ to remove." + echo "No $(pwd)/${BUILDDIR}/ to remove." fi ;; "--no-cd"|"-n") @@ -34,8 +40,8 @@ if [ -z "$NO_CD" ]; then pushd "$(dirname "$0" )" fi -mkdir -p build -pushd build +mkdir -p $BUILDDIR +pushd $BUILDDIR cleanup() { exit $SIGNAL; From 0eaec01225775db77e3fa49651237b4c4a79a669 Mon Sep 17 00:00:00 2001 From: Aiden Date: Sun, 25 Mar 2018 19:25:43 -0400 Subject: [PATCH 056/200] WIP - appendage header updates --- appendages/CMakeLists.txt | 2 +- appendages/include/appendages/roboclaw.hpp | 30 ++++++++++++++-------- appendages/src/roboclaw.cpp | 20 +++++++++------ 3 files changed, 32 insertions(+), 20 deletions(-) diff --git a/appendages/CMakeLists.txt b/appendages/CMakeLists.txt index 064b31c..0bcf991 100644 --- a/appendages/CMakeLists.txt +++ b/appendages/CMakeLists.txt @@ -9,7 +9,7 @@ add_library(${PROJECT_NAME} EXCLUDE_FROM_ALL ${${PROJECT_NAME}_SOURCES} ${${PROJ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) target_link_libraries(${PROJECT_NAME} fmt json spdlog cppfs - cmd_messenger misc) + cmd_messenger units misc) target_include_directories(${PROJECT_NAME} PUBLIC include) set_property(GLOBAL PROPERTY AppendageRoot ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index c650750..c6267a5 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -2,12 +2,12 @@ #define ROBOCLAW_HPP #include - +#include +#include #include - +#include #include "appendages/appendage.hpp" #include "appendages/appendage_factory.hpp" - #include namespace rip @@ -17,14 +17,19 @@ namespace rip class Roboclaw : public Appendage { public: - void SetSpeed(int32_t speed1, int32_t speed2); - void SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); - std::tuple ReadEncoders(); - void SetDuty(int16_t duty1, int16_t duty2); - void SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps); - + void drive(bool motor, int32_t speed) + void drive(int32_t speed1, int32_t speed2); + //void setSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); + std::tuple readEncoders(); + units::Distance readEncoder(bool motor) + std::tuple readEncoderSpeeds(); + //void setDuty(int16_t duty1, int16_t duty2); + void setVelocityPID(bool motor, float Kp, float Ki, float Kd, uint32_t qpps); + void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); + void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); + void resetEncoders(); /** - * Stop + * Stop! ^0^ */ virtual void stop() override; @@ -60,8 +65,11 @@ namespace rip std::shared_ptr m_read_encoders_result; std::shared_ptr m_set_duty; std::shared_ptr m_set_velocity_pid; + std::shared_ptr m_read_encoders_velocity; + std::shared_ptr m_read_encoders_velocity_result; + }; } } -#endif // ROBOCLAW_HPP \ No newline at end of file +#endif // ROBOCLAW_HPP diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index caa355d..bd23952 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -18,6 +18,8 @@ namespace rip m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoders(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoder_speeds(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoders_speeds_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_duty(createCommand("kSetDuty", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) { @@ -33,42 +35,44 @@ namespace rip return std::dynamic_pointer_cast(std::shared_ptr(new Roboclaw(config, command_map, device))); } - void Roboclaw::SetSpeed(int32_t speed1, int32_t speed2) + void Roboclaw::setSpeed(int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_speed, m_address, speed1, speed2); } - void Roboclaw::SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) + void Roboclaw::setSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); } - std::tuple Roboclaw::ReadEncoders() + std::tuple Roboclaw::readEncoders() { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read_encoders, m_address); return messenger.receive(m_read_encoders_result); } - void Roboclaw::SetDuty(int16_t duty1, int16_t duty2) + void Roboclaw::setDuty(int16_t duty1, int16_t duty2) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_duty, m_address, duty1, duty2); } - void Roboclaw::SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) + void Roboclaw::setVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_velocity_pid, m_address, motor, Kp, Ki, Kd, qpps); } - void Roboclaw::stop() { - SetSpeed(0, 0); + void Roboclaw::stop() + { + setSpeed(0, 0); } - bool Roboclaw::diagnostic() { + bool Roboclaw::diagnostic() + { // TODO } } From e9bc6b6458819300a205038105b00ef71066c674 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 19:33:32 -0400 Subject: [PATCH 057/200] Be more explicit in deps --- docker/rip_rpi.dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/rip_rpi.dockerfile b/docker/rip_rpi.dockerfile index c813a81..10f832c 100644 --- a/docker/rip_rpi.dockerfile +++ b/docker/rip_rpi.dockerfile @@ -44,7 +44,7 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ && DEBIAN_FRONTEND=noninteractive apt-get upgrade -y ' RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ DEBIAN_FRONTEND=noninteractive apt-get install -y \ - libc6-dev symlinks \ + libc6-dev symlinks libssl-dev libcrypto++-dev \ libeigen3-dev libsuitesparse-dev qt5-default \ bash zsh git vim tmux \ cmake lcov g++ time libssh-dev unzip \ From 54d7d21bea4c9434d6ed8044bc28a90e3b1cd7b2 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 19:43:46 -0400 Subject: [PATCH 058/200] Update before install again --- docker/rip_rpi.dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docker/rip_rpi.dockerfile b/docker/rip_rpi.dockerfile index 10f832c..8d40b30 100644 --- a/docker/rip_rpi.dockerfile +++ b/docker/rip_rpi.dockerfile @@ -43,7 +43,8 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ && DEBIAN_FRONTEND=noninteractive dpkg --configure -a \ && DEBIAN_FRONTEND=noninteractive apt-get upgrade -y ' RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ - DEBIAN_FRONTEND=noninteractive apt-get install -y \ + apt-get update \ + && DEBIAN_FRONTEND=noninteractive apt-get install -y \ libc6-dev symlinks libssl-dev libcrypto++-dev \ libeigen3-dev libsuitesparse-dev qt5-default \ bash zsh git vim tmux \ From 5d9c80238977cb0d805b760a5a7bdf1a18e41c1e Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Sun, 25 Mar 2018 18:50:42 -0500 Subject: [PATCH 059/200] Added checks for directory and file creation. --- CMakeLists.txt | 6 ++++- arduino_gen/src/arduino_gen.cpp | 40 ++++++++++++++++++++++++++------- 2 files changed, 37 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5446560..fcea426 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,11 @@ endif() # to correctly compile the codebase despite this line being in the # downstream CMake files. set (CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") + +option(GCC_DIAG_COLORS "Enable GCC Diagnostic Colors" ON) +if (GCC_DIAG_COLORS) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") +endif() LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/CMakeModules) diff --git a/arduino_gen/src/arduino_gen.cpp b/arduino_gen/src/arduino_gen.cpp index ae2313e..5b06014 100644 --- a/arduino_gen/src/arduino_gen.cpp +++ b/arduino_gen/src/arduino_gen.cpp @@ -117,56 +117,80 @@ namespace rip { device_folder.removeDirectoryRec(); } - device_folder.createDirectory(); + if (!device_folder.createDirectory()) + { + throw FileIoException(fmt::format("Could not create device folder: \"{}\"", device_folder.path())); + } device_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create src dir FileHandle source_folder = fs::open(fmt::format("{}/{}/{}", m_parent_folder, m_arduino, "src")); - source_folder.createDirectory(); + if (!source_folder.createDirectory()) + { + throw FileIoException(fmt::format("Could not create source folder: \"{}\"", source_folder.path())); + } source_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create ino file FileHandle source = fs::open(fmt::format("{0}/{1}/src/{1}.ino", m_parent_folder, m_arduino)); - source.writeFile(getArduinoCode()); + if (!source.writeFile(getArduinoCode())) + { + throw FileIoException(fmt::format("Could not create source file: \"{}\"", source.path())); + } source.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::OtherRead | FilePermissions::OtherWrite); // Create config file FileHandle json_config = fs::open(fmt::format("{0}/{1}/{1}.json", m_parent_folder, m_arduino)); - json_config.writeFile(config); + if (!json_config.writeFile(config)) + { + throw FileIoException(fmt::format("Could not create json config: \"{}\"", json_config.path())); + } json_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::OtherRead | FilePermissions::OtherWrite); // Create core file FileHandle core_config = fs::open(fmt::format("{0}/{1}/{1}_core.json", m_parent_folder, m_arduino)); - core_config.writeFile(getCoreConfig()); + if (!core_config.writeFile(getCoreConfig())) + { + throw FileIoException(fmt::format("Could not create core config: \"{}\"", core_config.path())); + } core_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::OtherRead | FilePermissions::OtherWrite); // Create platformio.ini FileHandle platformio_ini = fs::open(fmt::format("{}/{}/platformio.ini", m_parent_folder, m_arduino)); - platformio_ini.writeFile(platformio_str); + if (!platformio_ini.writeFile(platformio_str)) + { + throw FileIoException(fmt::format("Could not create platform.ini: \"{}\"", platformio_ini.path())); + } platformio_ini.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::OtherRead | FilePermissions::OtherWrite); // Create serial script FileHandle serial = fs::open(fmt::format("{0}/{1}/serial.sh", m_parent_folder, m_arduino)); - serial.writeFile(serial_str); + if (!serial.writeFile(serial_str)) + { + throw FileIoException(fmt::format("Could not create serial.sh: \"{}\"", serial.path())); + } serial.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create upload script FileHandle upload = fs::open(fmt::format("{0}/{1}/upload.sh", m_parent_folder, m_arduino)); - upload.writeFile(upload_str); + if (!upload.writeFile(upload_str)) + { + throw FileIoException(fmt::format("Could not create upload.sh: \"{}\"", upload.path())); + } upload.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); From 11c230a60e43c2c579a34b948c69dbd972f6ed48 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Sun, 25 Mar 2018 21:31:09 -0400 Subject: [PATCH 060/200] Add -p to non-testing version --- build-linux.sh | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/build-linux.sh b/build-linux.sh index 1f439ce..97e90ef 100755 --- a/build-linux.sh +++ b/build-linux.sh @@ -2,6 +2,7 @@ set -E BUILDDIR="build" +ENABLE_TESTING="ON" if [[ "$(dpkg --print-architecture)" == "armhf" ]]; then BUILDDIR="${BUILDDIR}_armhf" @@ -31,6 +32,11 @@ while [[ "$1" != "" ]]; do echo "Building project: $(pwd)" NO_CD="true" ;; + "--production"|"-p") + echo "Building production executables." + echo " -DENABLE_TESTING=OFF" + ENABLE_TESTING="OFF" + ;; esac shift done @@ -51,7 +57,7 @@ trap 'SIGNAL=$?;cleanup' ERR trap 'cleanup' SIGINT cmake .. \ - -DENABLE_TESTING=on \ + -DENABLE_TESTING=${ENABLE_TESTING} \ "$@" # From f7e560e1b2ff423bc69cc4956377348cde35e836 Mon Sep 17 00:00:00 2001 From: Aiden Date: Mon, 26 Mar 2018 11:52:41 -0400 Subject: [PATCH 061/200] added a diag function --- appendages/include/appendages/roboclaw.hpp | 5 ++-- appendages/src/roboclaw.cpp | 28 ++++++++++++++++++---- 2 files changed, 27 insertions(+), 6 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index c650750..782946d 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -2,7 +2,8 @@ #define ROBOCLAW_HPP #include - +#include +#include #include #include "appendages/appendage.hpp" @@ -64,4 +65,4 @@ namespace rip } } -#endif // ROBOCLAW_HPP \ No newline at end of file +#endif // ROBOCLAW_HPP diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index caa355d..5b860b9 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -1,9 +1,10 @@ #include "appendages/roboclaw.hpp" - +#include #include #include #include - +#include +#include #include #include "appendages/exceptions.hpp" @@ -68,8 +69,27 @@ namespace rip SetSpeed(0, 0); } - bool Roboclaw::diagnostic() { - // TODO + bool Roboclaw::diagnostic() + { + std::chrono::time_point start_time = std::chrono::system_clock::now(); + misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start"); + + misc::Logger::getInstance()->debug("Read encoders for 10s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) + { + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: ", std::get<0>(ReadEncoders()), std::get<0>(ReadEncoders()))); + } + misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); + SetDuty(16000, 16000); + start_time = std::chrono::system_clock::now(); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + {} + stop(); + misc::Logger::getInstance()->debug("Setting speed accel drive (5s)"); + SetSpeedAccel(12000, 12000, 12000); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + {} + stop(); } } } From 102e230216801e0c6eaed5effc343082a5e7c1ca Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Mon, 26 Mar 2018 22:26:39 -0400 Subject: [PATCH 062/200] WIP fixes for spine & diag --- .../include/framework/robot_base.hpp | 2 +- core/framework/include/framework/spine.hpp | 24 ++++++++++++++++--- core/framework/src/robot_base.cpp | 1 + core/framework/src/spine.cpp | 9 +++++-- core/utilities/diag/include/diag/diag.hpp | 15 +++++++++--- 5 files changed, 42 insertions(+), 9 deletions(-) diff --git a/core/framework/include/framework/robot_base.hpp b/core/framework/include/framework/robot_base.hpp index b8c8d0c..febb034 100644 --- a/core/framework/include/framework/robot_base.hpp +++ b/core/framework/include/framework/robot_base.hpp @@ -109,7 +109,7 @@ namespace rip std::string m_config_path; - friend class Diag; + friend class diag::Diag; }; } } diff --git a/core/framework/include/framework/spine.hpp b/core/framework/include/framework/spine.hpp index 1f2c3df..91a83cf 100644 --- a/core/framework/include/framework/spine.hpp +++ b/core/framework/include/framework/spine.hpp @@ -34,6 +34,11 @@ namespace rip { + namespace diag + { + class Diag; + } + namespace framework { /** @@ -60,6 +65,19 @@ namespace rip */ void loadDevices(const std::string& arduino_gen_folder, std::vector& device_names); + /** + * True if appendage is present + * @param appendage_name name of appendage to search for + * @returns boolean + */ + bool has(const std::string& appendage_name) { + if (m_appendages.find(appendage_name) == m_appendages.end()) + { + return false; + } + return true; + } + /** * @brief Returns the specified appendage * @@ -69,15 +87,14 @@ namespace rip * * @returns The specified appendage */ - template - std::shared_ptr get(const std::string& appendage_name) + std::shared_ptr get(const std::string& appendage_name) { if (m_appendages.find(appendage_name) == m_appendages.end()) { throw AppendageNotFound(fmt::format("Cannot find appendage named {}", appendage_name)); } - return std::dynamic_pointer_cast(m_appendages[appendage_name]); + return m_appendages[appendage_name]; } /** @@ -113,6 +130,7 @@ namespace rip std::map< std::string, std::shared_ptr > m_devices; std::map< std::string, std::shared_ptr > m_appendages; + friend class diag::Diag; }; // class Spine } } // namespace rip diff --git a/core/framework/src/robot_base.cpp b/core/framework/src/robot_base.cpp index 5186cba..41bebcb 100644 --- a/core/framework/src/robot_base.cpp +++ b/core/framework/src/robot_base.cpp @@ -50,6 +50,7 @@ namespace rip if(j.find("arduino_gen_home") != j.end()) { + misc::Logger::getInstance()->debug("Loading spine appendages..."); m_spine->loadDevices(j["arduino_gen_home"], devices); } diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index d18c856..0c318aa 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -5,6 +5,7 @@ #include #include +#include namespace rip { @@ -37,6 +38,7 @@ namespace rip { if (!canLoadDevice(arduino_gen_folder, device_name)) { + misc::Logger::getInstance()->error(fmt::format("Cannot load {}", device_name)); throw CannotLoadDevice(fmt::format("Cannot load {}", device_name)); } } @@ -46,8 +48,9 @@ namespace rip // Create devices for (const std::string& device_name : device_names) { + misc::Logger::getInstance()->debug(fmt::format("Loading appendage {}...", device_name)); m_devices[device_name] = std::make_shared(fmt::format("/dev/{}", device_name)); - loadConfig(m_devices[device_name], fmt::format("{}/{}/core.json", arduino_gen_folder, device_name)); + loadConfig(m_devices[device_name], fmt::format("{}/{}/{}_core.json", arduino_gen_folder, device_name, device_name)); } } @@ -75,15 +78,17 @@ namespace rip bool Spine::canLoadDevice(const std::string& arduino_gen_folder, const std::string& device_name) const { cppfs::FileHandle dev = cppfs::fs::open(fmt::format("/dev/{}", device_name)); - cppfs::FileHandle config = cppfs::fs::open(fmt::format("{}/{}/core.json", arduino_gen_folder, device_name)); + cppfs::FileHandle config = cppfs::fs::open(fmt::format("{}/{}/{}_core.json", arduino_gen_folder, device_name, device_name)); return dev.exists() && config.exists(); } void Spine::loadConfig(std::shared_ptr device, const std::string& path) { + misc::Logger::getInstance()->debug(fmt::format("Loading appendage config {} ...", path)); cppfs::FileHandle config_file = cppfs::fs::open(path); if (!config_file.exists()) { + misc::Logger::getInstance()->error(fmt::format("Cannot find appendage config file {}", path)); throw FileNotFound(fmt::format("Cannot find {}", path)); } diff --git a/core/utilities/diag/include/diag/diag.hpp b/core/utilities/diag/include/diag/diag.hpp index 87953eb..f959c70 100644 --- a/core/utilities/diag/include/diag/diag.hpp +++ b/core/utilities/diag/include/diag/diag.hpp @@ -25,7 +25,7 @@ namespace rip std::vector appendageNames() { std::vector rv; - for (auto const& element : m_robot->m_spine->appendages()) + for (auto const& element : m_robot->m_spine->m_appendages) { rv.push_back(element.first); } @@ -50,11 +50,20 @@ namespace rip // throw error } - void runAppendages(const std::string& name) + bool hasAppendage(const std::string& name) + { + return m_robot->m_spine->has(name); + } + + bool runAppendage(const std::string& name) { if (m_robot->m_spine->has(name)) { - m_robot->m_spine->get(name)->diagnostic(); + return m_robot->m_spine->get(name)->diagnostic(); + } + else + { + return false; } } From 31d5089f7738db1314f4578ec0c6b7f4a4c05eff Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 11:29:07 -0400 Subject: [PATCH 063/200] Working on spine fixes still --- appendages/include/appendages/exceptions.hpp | 2 +- appendages/src/appendage_factory.cpp | 2 +- core/framework/src/spine.cpp | 16 +++++++++++----- 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/appendages/include/appendages/exceptions.hpp b/appendages/include/appendages/exceptions.hpp index 56576f7..f8a0fa4 100644 --- a/appendages/include/appendages/exceptions.hpp +++ b/appendages/include/appendages/exceptions.hpp @@ -29,7 +29,7 @@ namespace rip NEW_EX(AppendageWithoutType) NEW_EX(AppendageWithoutLabel) NEW_EX(AppendageWithId) - NEW_EX(AppendageMissingField) + NEW_EX(AppendageMissingField) } } #endif // APPENDAGES_EXCEPTIONS_HPP diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index 1034978..07a01c8 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -46,7 +46,7 @@ namespace rip { registerAppendage("Digital Input", &DigitalInput::create); registerAppendage("Analog Input", &AnalogInput::create); - registerAppendage("Roboclaw", &Roboclaw::create); + registerAppendage("Roboclaw", &Roboclaw::create); } } } diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index 0c318aa..39fdb01 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -48,7 +48,7 @@ namespace rip // Create devices for (const std::string& device_name : device_names) { - misc::Logger::getInstance()->debug(fmt::format("Loading appendage {}...", device_name)); + misc::Logger::getInstance()->debug(fmt::format("Loading device {}...", device_name)); m_devices[device_name] = std::make_shared(fmt::format("/dev/{}", device_name)); loadConfig(m_devices[device_name], fmt::format("{}/{}/{}_core.json", arduino_gen_folder, device_name, device_name)); } @@ -106,15 +106,21 @@ namespace rip } std::shared_ptr factory = appendages::AppendageFactory::getInstance(); - nlohmann::json appendages; - for (nlohmann::json appendage : appendages) + nlohmann::json appendages_conf = config["appendages"]; + for (nlohmann::json appendage : appendages_conf) { if (appendage.find("type") == appendage.end()) { throw AppendageWithoutType(fmt::format("appendage missing type")); } - - m_appendages[appendage["type"]] = factory->makeAppendage(appendage, command_map, device); + std::string appendage_type = appendage["type"]; + if (appendage.find("label") == appendage.end()) + { + throw AppendageWithoutLabel(fmt::format("appendage of type {} has no label", appendage_type)) + } + std::string appendage_label = appendage["label"]; + misc::Logger::getInstance()->debug(fmt::format("Loading appendage {} of type {} ...", appendage_label, appendage_type)); + m_appendages[appendage_label] = factory->makeAppendage(appendage, command_map, device); } } } From 1ed0d03cb041a5ab0c7c8edfb85f39e68dd2df03 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 11:34:25 -0400 Subject: [PATCH 064/200] Perhaps we need to merge some exceptions --- core/framework/include/framework/exceptions.hpp | 1 + core/framework/src/spine.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/core/framework/include/framework/exceptions.hpp b/core/framework/include/framework/exceptions.hpp index 209cddc..4e9b60d 100644 --- a/core/framework/include/framework/exceptions.hpp +++ b/core/framework/include/framework/exceptions.hpp @@ -28,6 +28,7 @@ namespace rip NEW_EX(AppendageNotFound) NEW_EX(FileNotFound) NEW_EX(AppendageWithoutType) + NEW_EX(AppendageWithoutLabel) NEW_EX(CannotLoadDevice) } } diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index 39fdb01..cd02e9c 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -116,7 +116,7 @@ namespace rip std::string appendage_type = appendage["type"]; if (appendage.find("label") == appendage.end()) { - throw AppendageWithoutLabel(fmt::format("appendage of type {} has no label", appendage_type)) + throw AppendageWithoutLabel(fmt::format("appendage of type {} has no label", appendage_type)); } std::string appendage_label = appendage["label"]; misc::Logger::getInstance()->debug(fmt::format("Loading appendage {} of type {} ...", appendage_label, appendage_type)); From 3ee6a1b2a2b8717a4d5af6bc1b2d562392c49f70 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 27 Mar 2018 11:52:13 -0400 Subject: [PATCH 065/200] Prototypes [WIP][ci skip] --- appendages/include/appendages/roboclaw.hpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 2a562c0..4b470b1 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -21,13 +21,15 @@ namespace rip void drive(bool motor, int32_t speed) void drive(int32_t speed1, int32_t speed2); //void setSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); - std::tuple readEncoders(); - units::Distance readEncoder(bool motor) - std::tuple readEncoderSpeeds(); + std::array readEncoders(); + units::Distance readEncoder(bool motor); + std::array readEncoderSpeeds(); + units::Velocity readEncoderSpeed(bool motor); //void setDuty(int16_t duty1, int16_t duty2); void setVelocityPID(bool motor, float Kp, float Ki, float Kd, uint32_t qpps); void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); + std::array getBuffers(); void resetEncoders(); /** * Stop! ^0^ @@ -62,8 +64,14 @@ namespace rip std::shared_ptr m_set_speed; std::shared_ptr m_set_speed_accel; + std::shared_ptr m_set_speed_dist; + std::shared_ptr m_set_speed_accel_dist; + std::shared_ptr m_set_speed_accel_decel_dist; std::shared_ptr m_read_encoders; std::shared_ptr m_read_encoders_result; + std::shared_ptr m_reset_encoders; + std::shared_ptr m_get_buffers; + std::shared_ptr m_get_buffers_result; std::shared_ptr m_set_duty; std::shared_ptr m_set_velocity_pid; std::shared_ptr m_read_encoders_velocity; From 69c8e5bb10231612784672799847e8dd0f9d6c23 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 11:59:00 -0400 Subject: [PATCH 066/200] Working on Appendage Factory fixes --- appendages/include/appendages/exceptions.hpp | 2 +- appendages/src/appendage.cpp | 14 +++++++++++--- appendages/src/appendage_factory.cpp | 5 +++-- 3 files changed, 15 insertions(+), 6 deletions(-) diff --git a/appendages/include/appendages/exceptions.hpp b/appendages/include/appendages/exceptions.hpp index f8a0fa4..f5a96a6 100644 --- a/appendages/include/appendages/exceptions.hpp +++ b/appendages/include/appendages/exceptions.hpp @@ -28,7 +28,7 @@ namespace rip NEW_EX(CommandNotFound) NEW_EX(AppendageWithoutType) NEW_EX(AppendageWithoutLabel) - NEW_EX(AppendageWithId) + NEW_EX(AppendageWithoutId) NEW_EX(AppendageMissingField) } } diff --git a/appendages/src/appendage.cpp b/appendages/src/appendage.cpp index 081fa4d..ddc98a4 100644 --- a/appendages/src/appendage.cpp +++ b/appendages/src/appendage.cpp @@ -48,15 +48,23 @@ namespace rip if (config.find("label") == config.end()) { - throw AppendageWithoutLabel(fmt::format("appendage missing label")); + throw AppendageWithoutLabel(fmt::format("appendage of type {} is missing label", m_type)); } m_label = config["label"]; + if (config.find("id") == config.end() && config.find("index") == config.end()) + { + throw AppendageWithoutId(fmt::format("appendage labeled {} is missing an id or index", m_label)); + } + if (config.find("id") == config.end()) { - throw AppendageWithId(fmt::format("appendage missing id")); + m_id = config["index"]; + } + else + { + m_id = config["id"]; } - m_id = config["id"]; } } } diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index 07a01c8..1082d8f 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -30,8 +30,9 @@ namespace rip { throw AppendageWithoutType(fmt::format("appendage missing type")); } - - return m_constructors["type"](config, command_map, device); + std::string appendage_type = config["type"]; + misc::Logger::getInstance()->debug(fmt::format("Constructing a {} appendage...", appendage_type)); + return m_constructors[appendage_type](config, command_map, device); } void AppendageFactory::registerAppendage(const std::string& type, std::function(const nlohmann::json&, From 74c6a0d4e844a9ebf0d553f531931ab00cd8f05f Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 12:09:51 -0400 Subject: [PATCH 067/200] Words --- appendages/src/appendage_factory.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index 1082d8f..9b85b07 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -31,7 +31,7 @@ namespace rip throw AppendageWithoutType(fmt::format("appendage missing type")); } std::string appendage_type = config["type"]; - misc::Logger::getInstance()->debug(fmt::format("Constructing a {} appendage...", appendage_type)); + misc::Logger::getInstance()->debug(fmt::format("Factory: Constructing an appendage of type {} ...", appendage_type)); return m_constructors[appendage_type](config, command_map, device); } From 3c1e689c9b32ecebe5917916445670d68c79efda Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 13:19:52 -0400 Subject: [PATCH 068/200] Need to figure arduino_gen to fix this --- appendages/src/roboclaw.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 5b860b9..86df01a 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -24,7 +24,7 @@ namespace rip { if (config.find("address") == config.end()) { - throw AppendageMissingField(fmt::format("appendage roboclaw missing address")); + throw AppendageMissingField(fmt::format("Roboclaw: missing config field 'address'")); } m_address = config["address"]; } From 80bfc190ae7b9ea031d9dc8b838e8f95985d2123 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 14:18:48 -0400 Subject: [PATCH 069/200] Changing permissions settings in arduinogen Now setgid for robot group is used --- appendages/json/roboclaw.json | 5 +++-- arduino_gen/src/arduino_gen.cpp | 24 ++++++++++++------------ core/framework/src/spine.cpp | 2 +- 3 files changed, 16 insertions(+), 15 deletions(-) diff --git a/appendages/json/roboclaw.json b/appendages/json/roboclaw.json index b7553b1..a891552 100644 --- a/appendages/json/roboclaw.json +++ b/appendages/json/roboclaw.json @@ -1,4 +1,5 @@ { "serial": "string", - "baudrate": "int" -} \ No newline at end of file + "baudrate": "int", + "address": "int" +} diff --git a/arduino_gen/src/arduino_gen.cpp b/arduino_gen/src/arduino_gen.cpp index 5b06014..5305f0a 100644 --- a/arduino_gen/src/arduino_gen.cpp +++ b/arduino_gen/src/arduino_gen.cpp @@ -121,9 +121,9 @@ namespace rip { throw FileIoException(fmt::format("Could not create device folder: \"{}\"", device_folder.path())); } - device_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // device_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create src dir FileHandle source_folder = fs::open(fmt::format("{}/{}/{}", m_parent_folder, m_arduino, "src")); @@ -131,9 +131,9 @@ namespace rip { throw FileIoException(fmt::format("Could not create source folder: \"{}\"", source_folder.path())); } - source_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // source_folder.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create ino file FileHandle source = fs::open(fmt::format("{0}/{1}/src/{1}.ino", m_parent_folder, m_arduino)); @@ -143,7 +143,7 @@ namespace rip } source.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create config file FileHandle json_config = fs::open(fmt::format("{0}/{1}/{1}.json", m_parent_folder, m_arduino)); @@ -153,7 +153,7 @@ namespace rip } json_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create core file FileHandle core_config = fs::open(fmt::format("{0}/{1}/{1}_core.json", m_parent_folder, m_arduino)); @@ -163,7 +163,7 @@ namespace rip } core_config.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create platformio.ini FileHandle platformio_ini = fs::open(fmt::format("{}/{}/platformio.ini", m_parent_folder, m_arduino)); @@ -173,7 +173,7 @@ namespace rip } platformio_ini.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); // Create serial script FileHandle serial = fs::open(fmt::format("{0}/{1}/serial.sh", m_parent_folder, m_arduino)); @@ -183,7 +183,7 @@ namespace rip } serial.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); // Create upload script FileHandle upload = fs::open(fmt::format("{0}/{1}/upload.sh", m_parent_folder, m_arduino)); @@ -193,7 +193,7 @@ namespace rip } upload.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); } std::string ArduinoGen::getArduinoCode() diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index cd02e9c..724e33d 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -78,7 +78,7 @@ namespace rip bool Spine::canLoadDevice(const std::string& arduino_gen_folder, const std::string& device_name) const { cppfs::FileHandle dev = cppfs::fs::open(fmt::format("/dev/{}", device_name)); - cppfs::FileHandle config = cppfs::fs::open(fmt::format("{}/{}/{}_core.json", arduino_gen_folder, device_name, device_name)); + cppfs::FileHandle config = cppfs::fs::open(fmt::format("{0}/{1}/{1}_core.json", arduino_gen_folder, device_name)); return dev.exists() && config.exists(); } From ebb375980a4e8ff3820c5f9d26e9878fd088b48a Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 15:16:47 -0400 Subject: [PATCH 070/200] Diag roboclaw fix logging --- appendages/src/roboclaw.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 86df01a..4d0c79a 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -77,7 +77,7 @@ namespace rip misc::Logger::getInstance()->debug("Read encoders for 10s in a loop"); while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) { - misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: ", std::get<0>(ReadEncoders()), std::get<0>(ReadEncoders()))); + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); } misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); SetDuty(16000, 16000); From c44096ed1467d74f7d86bb8510485a4a22c611b5 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 15:41:36 -0400 Subject: [PATCH 071/200] Fix rest of AG unit tests --- arduino_gen/test/test_arduino_gen.cpp | 50 ++++++++++++++------------- 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/arduino_gen/test/test_arduino_gen.cpp b/arduino_gen/test/test_arduino_gen.cpp index e04bef8..62523c2 100644 --- a/arduino_gen/test/test_arduino_gen.cpp +++ b/arduino_gen/test/test_arduino_gen.cpp @@ -1548,30 +1548,31 @@ namespace rip // Check the file permissions #ifndef _WIN32 - EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); - EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // TODO setGID support + // EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(source.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(config.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(core.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(upload.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(serial.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(platformio.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); #endif // Change the platformio.ini file @@ -1857,30 +1858,31 @@ namespace rip // Check the file permissions #ifndef _WIN32 - EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); - EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | - FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // TODO setGID special bit support + // EXPECT_EQ(device_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + // EXPECT_EQ(source_folder.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | + // FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | + // FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(source.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(config.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(core.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); EXPECT_EQ(upload.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(serial.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::UserExec | FilePermissions::GroupRead | FilePermissions::GroupWrite | FilePermissions::GroupExec | - FilePermissions::OtherRead | FilePermissions::OtherWrite | FilePermissions::OtherExec); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite | FilePermissions::OtherExec); EXPECT_EQ(platformio.permissions(), FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | - FilePermissions::OtherRead | FilePermissions::OtherWrite); + FilePermissions::OtherRead);// | FilePermissions::OtherWrite); #endif // Cleanup From ade2ab4f773bce0dca06732660331b45ff79287e Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 15:57:11 -0400 Subject: [PATCH 072/200] Copy Ultrasonic and Servo --- appendages/json/servo.json | 3 +++ appendages/json/ultrasonic.json | 4 ++++ appendages/xml/servo.xml | 29 +++++++++++++++++++++++++++++ appendages/xml/ultrasonic.xml | 26 ++++++++++++++++++++++++++ 4 files changed, 62 insertions(+) create mode 100644 appendages/json/servo.json create mode 100644 appendages/json/ultrasonic.json create mode 100644 appendages/xml/servo.xml create mode 100644 appendages/xml/ultrasonic.xml diff --git a/appendages/json/servo.json b/appendages/json/servo.json new file mode 100644 index 0000000..8bd4e95 --- /dev/null +++ b/appendages/json/servo.json @@ -0,0 +1,3 @@ +{ + "pin": "int" +} diff --git a/appendages/json/ultrasonic.json b/appendages/json/ultrasonic.json new file mode 100644 index 0000000..1367142 --- /dev/null +++ b/appendages/json/ultrasonic.json @@ -0,0 +1,4 @@ +{ + "triggerPin": "int", + "echoPin": "int" +} diff --git a/appendages/xml/servo.xml b/appendages/xml/servo.xml new file mode 100644 index 0000000..a6f78d2 --- /dev/null +++ b/appendages/xml/servo.xml @@ -0,0 +1,29 @@ + + + Servo.h + + + + + + + + + + servos[$i$].attach($pin$); + + + // Servo pin: $pin$ + + + + + + if (!servos[indexNum].attached()) { + servos[indexNum].attach(servo_pins[indexNum]); + } + servos[indexNum].write(value); + + + + diff --git a/appendages/xml/ultrasonic.xml b/appendages/xml/ultrasonic.xml new file mode 100644 index 0000000..52d927b --- /dev/null +++ b/appendages/xml/ultrasonic.xml @@ -0,0 +1,26 @@ + + + NewPing.h + + + + + + + + + + // Ultrasonic triggerPin: $triggerPin$ + + + // Ultrasonic echoPin: $echoPin$ + + + + + + rv = sonar[indexNum].ping_cm(); + + + + From 1a2b2c69ec8ca76089df0ca47612066b8b27b806 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 16:16:19 -0400 Subject: [PATCH 073/200] Factory: check that appendage is available --- appendages/include/appendages/exceptions.hpp | 1 + appendages/src/appendage_factory.cpp | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/appendages/include/appendages/exceptions.hpp b/appendages/include/appendages/exceptions.hpp index f5a96a6..e2a7b23 100644 --- a/appendages/include/appendages/exceptions.hpp +++ b/appendages/include/appendages/exceptions.hpp @@ -30,6 +30,7 @@ namespace rip NEW_EX(AppendageWithoutLabel) NEW_EX(AppendageWithoutId) NEW_EX(AppendageMissingField) + NEW_EX(AppendageNotImplemented) } } #endif // APPENDAGES_EXCEPTIONS_HPP diff --git a/appendages/src/appendage_factory.cpp b/appendages/src/appendage_factory.cpp index 9b85b07..40b4c27 100644 --- a/appendages/src/appendage_factory.cpp +++ b/appendages/src/appendage_factory.cpp @@ -6,6 +6,8 @@ #include "appendages/digital_input.hpp" #include "appendages/analog_input.hpp" #include "appendages/roboclaw.hpp" +#include "appendages/servo.hpp" +#include "appendages/ultrasonic.hpp" #include "appendages/exceptions.hpp" @@ -31,6 +33,11 @@ namespace rip throw AppendageWithoutType(fmt::format("appendage missing type")); } std::string appendage_type = config["type"]; + if (m_constructors.find(appendage_type) == m_constructors.end()) + { + // we probably don't have this appendage type available to the factory + throw AppendageNotImplemented(fmt::format("Factory: no Constructor for appendage type {} !", appendage_type)); + } misc::Logger::getInstance()->debug(fmt::format("Factory: Constructing an appendage of type {} ...", appendage_type)); return m_constructors[appendage_type](config, command_map, device); } @@ -48,6 +55,8 @@ namespace rip registerAppendage("Digital Input", &DigitalInput::create); registerAppendage("Analog Input", &AnalogInput::create); registerAppendage("Roboclaw", &Roboclaw::create); + registerAppendage("Servo", &Servo::create); + registerAppendage("Ultrasonic", &Ultrasonic::create); } } } From a4552d5c5f6e05311880025ab9c9b61aeebbc875 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 16:38:42 -0400 Subject: [PATCH 074/200] Ultrasonic diag --- appendages/src/ultrasonic.cpp | 19 ++++++++++++++++--- core/framework/src/robot_base.cpp | 2 +- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/appendages/src/ultrasonic.cpp b/appendages/src/ultrasonic.cpp index 20270b5..4976b38 100644 --- a/appendages/src/ultrasonic.cpp +++ b/appendages/src/ultrasonic.cpp @@ -3,6 +3,10 @@ #include #include #include +#include +#include + +#include #include @@ -12,8 +16,8 @@ namespace rip { Ultrasonic::Ultrasonic(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device) - , m_read(createCommand("kUltrasonicRead", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) - , m_read_result(createCommand("kUltrasonicReadResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_read(createCommand("kReadUltrasonic", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_read_result(createCommand("kReadUltrasonicResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) { } @@ -26,11 +30,20 @@ namespace rip void Ultrasonic::stop() { - + // don't need to do anything. } bool Ultrasonic::diagnostic() { + std::chrono::time_point start_time = std::chrono::system_clock::now(); + misc::Logger::getInstance()->debug("Reading the ultrasonic value for 10s."); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) + { + misc::Logger::getInstance()->debug(fmt::format("Distance: {} cm", read().to(units::cm))); + } + + misc::Logger::getInstance()->debug("Ultrasonic diag finished."); return true; } diff --git a/core/framework/src/robot_base.cpp b/core/framework/src/robot_base.cpp index 41bebcb..ba9195d 100644 --- a/core/framework/src/robot_base.cpp +++ b/core/framework/src/robot_base.cpp @@ -75,7 +75,7 @@ namespace rip void RobotBase::stop() { - misc::Logger::getInstance()->debug("Stoping the robot..."); + misc::Logger::getInstance()->debug("Stopping the robot..."); m_running = false; } From 5b5c6381b7545340ab2b1fdd3b29e22fe6980b99 Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Tue, 27 Mar 2018 17:21:53 -0500 Subject: [PATCH 075/200] Added core field to appendages --- arduino_gen/include/arduino_gen/appendage.hpp | 5 +++- arduino_gen/src/appendage.cpp | 23 +++++++++++++++++-- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/arduino_gen/include/arduino_gen/appendage.hpp b/arduino_gen/include/arduino_gen/appendage.hpp index 9bc0c21..07460b3 100644 --- a/arduino_gen/include/arduino_gen/appendage.hpp +++ b/arduino_gen/include/arduino_gen/appendage.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include @@ -89,13 +90,15 @@ namespace rip * @exception ParameterInvalid This exception is thrown if a parameter is missing, * included but not in the template, or has the wrong type. */ - void testType() const; + void testType(); nlohmann::json m_data; static std::map< std::string, std::map< std::string, std::string > > m_type_cache; std::string m_appendage_data_folder; + + std::vector m_core_fields; }; } } diff --git a/arduino_gen/src/appendage.cpp b/arduino_gen/src/appendage.cpp index 5ea1382..ea7d80f 100644 --- a/arduino_gen/src/appendage.cpp +++ b/arduino_gen/src/appendage.cpp @@ -109,7 +109,7 @@ namespace rip } } - void Appendage::testType() const + void Appendage::testType() { std::string type = getType(); @@ -151,7 +151,21 @@ namespace rip (*type_template.createInputStream()) >> j; std::map< std::string, std::string > temp; for (nlohmann::json::iterator it = j.begin(); it != j.end(); ++it) { - temp[it.key()] = it.value(); + if (it.key() == "core") + { + for (std::string core_key : it.value()) + { + if (m_data.find(core_key) == m_data.end()) + { + throw AppendageDataException(fmt::format("{} missing core field {}", label, core_key)); + } + m_core_fields.emplace_back(core_key); + } + } + else + { + temp[it.key()] = it.value(); + } } m_type_cache[type] = temp; } @@ -234,6 +248,11 @@ namespace rip json["label"] = m_data["label"]; json["index"] = index; + for (std::string core_key : m_core_fields) + { + json[core_key] = m_data[core_key]; + } + return json; } } From 75adf743dc856976bf53a7296a1599d5e4ef81ae Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 27 Mar 2018 18:27:39 -0400 Subject: [PATCH 076/200] Updated stuff, activate collaboration --- appendages/include/appendages/roboclaw.hpp | 87 ++++++++++--- appendages/src/roboclaw.cpp | 137 ++++++++++++++++++++- appendages/xml/roboclaw.xml | 46 ++++++- 3 files changed, 245 insertions(+), 25 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 4b470b1..6e2b00b 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -18,19 +18,49 @@ namespace rip class Roboclaw : public Appendage { public: - void drive(bool motor, int32_t speed) - void drive(int32_t speed1, int32_t speed2); - //void setSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); + void setM1Speed(int32_t speed); + void setM2Speed(int32_t speed); + void setM1M2Speed(int32_t speed); + + void setM1SpeedAccel(uint32_t accel, int32_t speed); + void setM2SpeedAccel(int32_t accel, int32_t speed); + void setM1M2SpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); + + void setM1SpeedDist(int32_t speed, uint32_t distance); + void setM2SpeedDist(int32_t speed, uint32_t distance); + void setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2); + + void setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance); + void setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance); + void setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2); + + void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); + void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); + void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); + + int32_t readM1Encoder(); + int32_t readM2Encoder(); + std::array readM1M2Encoders(); std::array readEncoders(); units::Distance readEncoder(bool motor); + + int32_t readM1EncoderSpeed(); + int32_t readM2EncoderSpeed(); + int32_t readM1M2EncoderSpeed(); std::array readEncoderSpeeds(); units::Velocity readEncoderSpeed(bool motor); - //void setDuty(int16_t duty1, int16_t duty2); + + void setM1Duty(int16_t duty); + void setM2Duty(int16_t duty); + void setDuties(int16_t duty1, int16_t duty2); + void setDuty(bool motor, int16_t duty); + void setVelocityPID(bool motor, float Kp, float Ki, float Kd, uint32_t qpps); - void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); - void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); std::array getBuffers(); void resetEncoders(); + //void setSpeeds(int32_t speed1, uint32_t speed2); + void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); + void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); /** * Stop! ^0^ */ @@ -61,20 +91,49 @@ namespace rip Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); uint8_t m_address; + double m_ticks_per_rev; + units::Distance m_wheel_radius; + + std::shared_ptr m_set_m1_speed; + std::shared_ptr m_set_m2_speed; + std::shared_ptr m_set_m1m2_speed; + + std::shared_ptr m_set_m1_speed_accel; + std::shared_ptr m_set_m2_speed_accel; + std::shared_ptr m_set_m1m2_speed_accel; + + std::shared_ptr m_set_m1_speed_dist; + std::shared_ptr m_set_m2_speed_dist; + std::shared_ptr m_set_m1m2_speed_dist; + + std::shared_ptr m_set_m1_speed_accel_dist; + std::shared_ptr m_set_m2_speed_accel_dist; + std::shared_ptr m_set_m1m2_speed_accel_dist; - std::shared_ptr m_set_speed; - std::shared_ptr m_set_speed_accel; - std::shared_ptr m_set_speed_dist; - std::shared_ptr m_set_speed_accel_dist; - std::shared_ptr m_set_speed_accel_decel_dist; - std::shared_ptr m_read_encoders; + std::shared_ptr m_set_m1_speed_accel_decel_dist; + std::shared_ptr m_set_m2_speed_accel_decel_dist; + std::shared_ptr m_set_m1m2_speed_accel_decel_dist; + + std::shared_ptr m_read_m1_encoder; + std::shared_ptr m_read_m2_encoder; + std::shared_ptr m_read_m1m2_encoder; + std::shared_ptr m_read_encoder_result; std::shared_ptr m_read_encoders_result; + std::shared_ptr m_reset_encoders; std::shared_ptr m_get_buffers; std::shared_ptr m_get_buffers_result; - std::shared_ptr m_set_duty; + + std::shared_ptr m_set_m1_duty; + std::shared_ptr m_set_m2_duty; + std::shared_ptr m_set_m1m2_duty; + std::shared_ptr m_set_velocity_pid; - std::shared_ptr m_read_encoders_velocity; + + std::shared_ptr m_read_m1_encoder_velocity; + std::shared_ptr m_read_m2_encoder_velocity; + std::shared_ptr m_read_m1m2_encoder_velocity; + std::shared_ptr m_read_encoder_velocity_result; std::shared_ptr m_read_encoders_velocity_result; }; diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index c603a02..ace9990 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -17,6 +17,9 @@ namespace rip : Appendage(config, device), m_set_speed(createCommand("kSetSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_speed_dist(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoders(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoder_speeds(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), @@ -29,6 +32,17 @@ namespace rip throw AppendageMissingField(fmt::format("appendage roboclaw missing address")); } m_address = config["address"]; + if (config.find("wheel_radius") == config.end()) + { + throw AppendageMissingField(fmt::format("appendage roboclaw missing wheel radius")); + } + m_wheel_radius = config.at("wheel_radius"); + if (config.find("ticks_per_rev") == config.end()) + { + throw AppendageMissingField(fmt::format("appendage roboclaw missing address")); + } + m_ticks_per_rev = config.at("ticks_per_rev"); + } std::shared_ptr Roboclaw::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) @@ -36,6 +50,16 @@ namespace rip return std::dynamic_pointer_cast(std::shared_ptr(new Roboclaw(config, command_map, device))); } + units::Distance Roboclaw::readEncoder(bool motor) + { + cmdmessenger::ArduinoCmdMessenger messenger; + } + + units::Velocity Roboclaw::readEncoderSpeed(bool motor) + { + cmdmessenger::ArduinoCmdMessenger messenger; + } + void Roboclaw::setSpeed(int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; @@ -48,13 +72,116 @@ namespace rip messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); } - std::tuple Roboclaw::readEncoders() + std::array Roboclaw::readEncoders() { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read_encoders, m_address); return messenger.receive(m_read_encoders_result); } + std::array Roboclaw::readEncoderSpeeds() + { + cmdmessenger::ArduinoCmdMessenger messenger; + } + + void Roboclaw::setDynamic(bool motor, const MotorDynamics& dynamics, bool respectBuffer) + { + cmdmessenger::ArduinoCmdMessenger messenger; + int32_t speed; + uint32_t accel, dist, decel; + + switch(dynamics.getDType()) + { + case MotorDynamics::DType::kNone: + { + return; + } + case MotorDynamics::DType::kSpeed: + { + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + messenger.send( + m_device, m_set_speed, m_address, speed1, speed2); + + } + case MotorDynamics::DType::kSpeedAccel: + { + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / (m_wheel_radius * M_PI * 2)).to(1 / (units::s * units::s)) * m_ticks_per_rev); + messenger.send( + m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + } + case MotorDynamics::DType::kSpeedDist: + { + + } + case MotorDynamics::DType::kSpeedAccelDist: + { + + } + case MotorDynamics::DType::kSpeedAccelDecelDist: + { + + } + default: + { + assert(false); + } + } + } + + void Roboclaw::setDynamics(const MotorDynamics& dynamics, bool respectBuffer); + { + cmdmessenger::ArduinoCmdMessenger messenger; + int32_t speed; + uint32_t accel, dist, decel; + + switch(dynamics.getDType()) + { + case MotorDynamics::DType::kNone: + { + + } + case MotorDynamics::DType::kSpeed: + { + + } + case MotorDynamics::DType::kSpeedAccel: + { + messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + } + case MotorDynamics::DType::kSpeedDist: + { + + } + case MotorDynamics::DType::kSpeedDist: + { + + } + case MotorDynamics::DType::kSpeedAccelDist: + { + + } + case MotorDynamics::DType::kSpeedAccelDecelDist: + { + + } + default: + { + assert(false); + } + } + } + + std::array Roboclaw::getBuffers(); + { + cmdmessenger::ArduinoCmdMessenger messenger; + } + void Roboclaw::setDuty(int16_t duty1, int16_t duty2) { cmdmessenger::ArduinoCmdMessenger messenger; @@ -69,14 +196,12 @@ namespace rip void Roboclaw::stop() { - setSpeed(0, 0); + setDuty(0, 0); } bool Roboclaw::diagnostic() { -<<<<<<< HEAD - // TODO -======= + /* std::chrono::time_point start_time = std::chrono::system_clock::now(); misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start"); @@ -96,7 +221,7 @@ namespace rip while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) {} stop(); ->>>>>>> robo/integration-39 + */ } } } diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 96c6936..63056b8 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -12,7 +12,7 @@ roboclaws[$i$].begin($baudrate$); - + @@ -23,7 +23,43 @@ } - + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); + } + + + in + + + + + + if (!roboclaws[indexNum].SpeedDistM1M2(address, accel, speed1, speed2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); + } + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) // TODO: Check return value + { + cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); + } + + + @@ -46,7 +82,7 @@ } - + @@ -66,7 +102,7 @@ bool result; - + switch (motor) { case 1: @@ -79,7 +115,7 @@ result = false; break; } - + if (!result) // TODO: Double check return values { cmdMessenger.sendBinCmd(kError, kSetVelocityPID); From ff1271dbe39950e594fa30722c3d71faacdc03be Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Tue, 27 Mar 2018 17:28:17 -0500 Subject: [PATCH 077/200] Added core field address to roboclaw --- appendages/json/roboclaw.json | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/appendages/json/roboclaw.json b/appendages/json/roboclaw.json index b7553b1..dcb4c0e 100644 --- a/appendages/json/roboclaw.json +++ b/appendages/json/roboclaw.json @@ -1,4 +1,8 @@ { "serial": "string", - "baudrate": "int" + "baudrate": "int", + "address": "int", + "core": [ + "address" + ] } \ No newline at end of file From 66309a841feed05097caf5ae73c04419521e4372 Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Tue, 27 Mar 2018 19:06:33 -0400 Subject: [PATCH 078/200] pose --- core/navigation/CMakeLists.txt | 2 +- core/navigation/localization/CMakeLists.txt | 27 - .../include/localization/action_model.hpp | 21 - .../extended_information_filter.hpp | 76 --- .../localization/extended_kalman_filter.hpp | 125 ----- .../localization/information_filter.hpp.hpp | 70 --- .../localization/markov_action_chain.hpp | 182 ------ .../probability/bayes_distribution.hpp | 95 ---- .../probability/conditional_map.hpp | 52 -- .../continuous/gaussian_canonical.hpp | 134 ----- .../continuous/gaussian_moment.hpp | 132 ----- .../localization/probability/data/matrix.hpp | 81 --- .../data/matrix_vector_operators.hpp | 25 - .../localization/probability/data/vector.hpp | 73 --- .../probability/data/vector_range.hpp | 64 --- .../discrete/conditional_value_map.hpp | 170 ------ .../discrete/discrete_conditional.hpp | 36 -- .../discrete/discrete_distribution.hpp | 48 -- .../discrete/distribution_counter.hpp | 65 --- .../discrete/distribution_value_map.hpp | 118 ---- .../probability/discrete/markov_chain.hpp | 158 ------ .../independent_pair_distribution.hpp | 54 -- .../probability/random_conditional.hpp | 29 - .../probability/random_distribution.hpp | 31 -- .../probability/random_uniform.hpp | 54 -- .../probability/ranged_uniform.hpp | 51 -- .../src/probability/data/matrix.cpp | 524 ------------------ .../data/matrix_vector_operators.cpp | 72 --- .../src/probability/data/vector.cpp | 209 ------- core/navigation/localization/test/main.cpp | 8 - .../test/test_bayes_distribution.cpp | 111 ---- .../test/test_conditional_map.hpp.cpp | 38 -- .../test/test_conditional_value_map.cpp | 132 ----- .../test_independent_pair_distribution.cpp | 75 --- .../test/test_random_distribution.cpp | 46 -- .../localization/test/test_ranged_uniform.cpp | 44 -- core/navigation/pose/CMakeLists.txt | 32 ++ .../pose/cmake_modules/FindEigen3.cmake | 87 +++ .../pose/include/pose/action_model.hpp | 26 + .../include/pose/extended_kalman_filter.hpp | 136 +++++ .../include/pose}/gaussian_action_model.hpp | 18 +- .../pose/include/pose/gaussian_moment.hpp | 146 +++++ .../include/pose/gaussian_sensor_model.hpp} | 20 +- .../include/pose}/kalman_filter.hpp | 101 ++-- core/navigation/pose/include/pose/matrix.hpp | 74 +++ .../include/pose/matrix_vector_operators.hpp | 21 + .../pose/include/pose/random_distribution.hpp | 29 + .../include/pose}/sensor_model.hpp | 13 +- .../include/pose/unscented_kalman_filter.hpp | 188 +++++++ core/navigation/pose/include/pose/vector.hpp | 85 +++ .../pose/include/pose/vector_range.hpp | 61 ++ core/navigation/pose/src/matrix.cpp | 499 +++++++++++++++++ .../pose/src/matrix_vector_operators.cpp | 69 +++ core/navigation/pose/src/vector.cpp | 205 +++++++ 54 files changed, 1742 insertions(+), 3300 deletions(-) delete mode 100644 core/navigation/localization/CMakeLists.txt delete mode 100644 core/navigation/localization/include/localization/action_model.hpp delete mode 100644 core/navigation/localization/include/localization/extended_information_filter.hpp delete mode 100644 core/navigation/localization/include/localization/extended_kalman_filter.hpp delete mode 100644 core/navigation/localization/include/localization/information_filter.hpp.hpp delete mode 100644 core/navigation/localization/include/localization/markov_action_chain.hpp delete mode 100644 core/navigation/localization/include/localization/probability/bayes_distribution.hpp delete mode 100644 core/navigation/localization/include/localization/probability/conditional_map.hpp delete mode 100644 core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp delete mode 100644 core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp delete mode 100644 core/navigation/localization/include/localization/probability/data/matrix.hpp delete mode 100644 core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp delete mode 100644 core/navigation/localization/include/localization/probability/data/vector.hpp delete mode 100644 core/navigation/localization/include/localization/probability/data/vector_range.hpp delete mode 100644 core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp delete mode 100644 core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp delete mode 100644 core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp delete mode 100644 core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp delete mode 100644 core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp delete mode 100644 core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp delete mode 100644 core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp delete mode 100644 core/navigation/localization/include/localization/probability/random_conditional.hpp delete mode 100644 core/navigation/localization/include/localization/probability/random_distribution.hpp delete mode 100644 core/navigation/localization/include/localization/probability/random_uniform.hpp delete mode 100644 core/navigation/localization/include/localization/probability/ranged_uniform.hpp delete mode 100644 core/navigation/localization/src/probability/data/matrix.cpp delete mode 100644 core/navigation/localization/src/probability/data/matrix_vector_operators.cpp delete mode 100644 core/navigation/localization/src/probability/data/vector.cpp delete mode 100644 core/navigation/localization/test/main.cpp delete mode 100644 core/navigation/localization/test/test_bayes_distribution.cpp delete mode 100644 core/navigation/localization/test/test_conditional_map.hpp.cpp delete mode 100644 core/navigation/localization/test/test_conditional_value_map.cpp delete mode 100644 core/navigation/localization/test/test_independent_pair_distribution.cpp delete mode 100644 core/navigation/localization/test/test_random_distribution.cpp delete mode 100644 core/navigation/localization/test/test_ranged_uniform.cpp create mode 100644 core/navigation/pose/CMakeLists.txt create mode 100644 core/navigation/pose/cmake_modules/FindEigen3.cmake create mode 100644 core/navigation/pose/include/pose/action_model.hpp create mode 100644 core/navigation/pose/include/pose/extended_kalman_filter.hpp rename core/navigation/{localization/include/localization => pose/include/pose}/gaussian_action_model.hpp (67%) create mode 100644 core/navigation/pose/include/pose/gaussian_moment.hpp rename core/navigation/{localization/include/localization/gaussian_sensor_model.hpp.hpp => pose/include/pose/gaussian_sensor_model.hpp} (60%) rename core/navigation/{localization/include/localization => pose/include/pose}/kalman_filter.hpp (63%) create mode 100644 core/navigation/pose/include/pose/matrix.hpp create mode 100644 core/navigation/pose/include/pose/matrix_vector_operators.hpp create mode 100644 core/navigation/pose/include/pose/random_distribution.hpp rename core/navigation/{localization/include/localization => pose/include/pose}/sensor_model.hpp (65%) create mode 100644 core/navigation/pose/include/pose/unscented_kalman_filter.hpp create mode 100644 core/navigation/pose/include/pose/vector.hpp create mode 100644 core/navigation/pose/include/pose/vector_range.hpp create mode 100644 core/navigation/pose/src/matrix.cpp create mode 100644 core/navigation/pose/src/matrix_vector_operators.cpp create mode 100644 core/navigation/pose/src/vector.cpp diff --git a/core/navigation/CMakeLists.txt b/core/navigation/CMakeLists.txt index 0807fe9..1cba7d6 100644 --- a/core/navigation/CMakeLists.txt +++ b/core/navigation/CMakeLists.txt @@ -6,4 +6,4 @@ add_subdirectory(path_planner) add_subdirectory(actions) add_subdirectory(drivetrains) add_subdirectory(navx) -add_subdirectory(localization) +add_subdirectory(pose) diff --git a/core/navigation/localization/CMakeLists.txt b/core/navigation/localization/CMakeLists.txt deleted file mode 100644 index 3c2a281..0000000 --- a/core/navigation/localization/CMakeLists.txt +++ /dev/null @@ -1,27 +0,0 @@ -cmake_minimum_required(VERSION 3.1) -message("Building localization") -# define the project -project(localization) - -# create and configure the library target -file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES src/**/**.cpp) -file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS include/**/**.hpp) -add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) -set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) -# target_link_libraries(${PROJECT_NAME} fmt json units spdlog motor_controllers navx misc) -target_include_directories(${PROJECT_NAME} PUBLIC include) - -# if(ENABLE_TESTING) - # Get test source files and test data files - file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") - #file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") - - # Copy test data to outdir/test - #make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) - - # Create localization_test executable, set to c++11, link localization library and google test libraries - add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES})# ${testDataOutputs}) - set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) - target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) - target_link_libraries(${PROJECT_NAME}_test gmock gtest googletest_rip_macros) -# endif() diff --git a/core/navigation/localization/include/localization/action_model.hpp b/core/navigation/localization/include/localization/action_model.hpp deleted file mode 100644 index fc3d165..0000000 --- a/core/navigation/localization/include/localization/action_model.hpp +++ /dev/null @@ -1,21 +0,0 @@ -#ifndef ACTION_MODEL_HPP -#define ACTION_MODEL_HPP - -namespace rip -{ - namespace navigation - { - namespace localization - { - template - class ActionModel - { - public: - ActionModel() = default; - virtual ~ActionModel() = default; - }; - } - } -} - -#endif // ACTION_MODEL_HPP diff --git a/core/navigation/localization/include/localization/extended_information_filter.hpp b/core/navigation/localization/include/localization/extended_information_filter.hpp deleted file mode 100644 index e39e2ff..0000000 --- a/core/navigation/localization/include/localization/extended_information_filter.hpp +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef EXTENDED_INFORMATION_FILTER_HPP -#define EXTENDED_INFORMATION_FILTER_HPP - -#include "extended_kalman_filter.hpp" -#include "probability/continuous/gaussian_canonical.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - template - class ExtendedInformationFilter - { - using probability::Matrix; - using probability::Vector; - using probability::GaussianCanonical; - public: - ExtendedInformationFilter() {} - virtual ~ExtendedInformationFilter() {} - - std::shared_ptr< GaussianCanonical > marginalize( - const ExtendedKalmanActionModel& model, - const U& action, - const GaussianCanonical& belief) - { - - - const X& mu = belief.getMu(); - const Matrix& Sigma = belief.inversePrecision(); - - Matrix G = model.getStateJacobian(action, mu); - Matrix Gt = G.transpose(); - Matrix R = model.getError(action, mu).inverse(); - Matrix nextOmega = (G * Sigma * Gt + R).inverse(); - - X nextXi; - nextXi.set(nextOmega * model.getMean(action, mu)); - - return std::make_shared>(nextXi, nextOmega); - } - - std::shared_ptr> - bayesianInference( - const ExtendedKalmanSensorModel& model, - const Z& observation, - const probability::continuous::GaussianCanonical& belief) - { - using probability::Matrix; - using probability::Vector; - using probability::continuous::GaussianCanonical; - - const X& mu = belief.getMu(); - const Matrix& Omega = belief.precision(); - - Matrix H = model.getJacobian(mu); - Matrix Ht = H.transpose(); - Matrix Q = model.getError(observation); - Matrix QInv = Q.inverse(); - Matrix nextOmega = Omega + Ht * QInv * H; - - const Vector& xi = belief.information(); - Z expectedObservation = model.getMean(mu); - - Vector nextXi = xi + Ht * QInv * (observation - expectedObservation + H * mu); - - return std::make_shared>(nextXi, nextOmega); - } - }; - - } - } -} - -#endif // EXTENDED_INFORMATION_FILTER_HPP diff --git a/core/navigation/localization/include/localization/extended_kalman_filter.hpp b/core/navigation/localization/include/localization/extended_kalman_filter.hpp deleted file mode 100644 index 22ae638..0000000 --- a/core/navigation/localization/include/localization/extended_kalman_filter.hpp +++ /dev/null @@ -1,125 +0,0 @@ -#ifndef EXTENDED_KALMAN_FILTER_HPP -#define EXTENDED_KALMAN_FILTER_HPP - -#include "probability/data/matrix.hpp" -#include "probability/continuous/gaussian_moment.hpp" - - -namespace rip -{ - namespace navigation - { - namespace localization - { - - using Matrix = probability::Matrix; - - template - class ExtendedKalmanActionModel : public GaussianActionModel - { - public: - ExtendedKalmanActionModel() {} - virtual ~ExtendedKalmanActionModel() {} - - virtual Matrix GetStateJacobian(const U& action, const X& state) const = 0; - virtual Matrix GetActionJacobian(const U& action, const X& state) const = 0; - }; - - template - class ExtendedKalmanSensorModel : public GaussianSensorModel - { - public: - ExtendedKalmanSensorModel() {} - virtual ~ExtendedKalmanSensorModel() {} - - virtual Matrix GetJacobian(const X& x) const = 0; - }; - - template - class ExtendedKalmanFilter - { - using probability::GaussianMoment; - using probability::Vector; - public: - ExtendedKalmanFilter() {} - virtual ~ExtendedKalmanFilter() {} - - std::shared_ptr< GaussianMoment > marginalize( - const ExtendedKalmanActionModel& model, - const U& action, - const GaussianMoment& belief) - { - - X x0 = belief.mean(); - Matrix e0 = belief.covariance(); - - // time update - Matrix g = model.getStateJacobian(action, x0); - Matrix gt = g.transpose(); - Matrix r = model.getError(action, x0); - - X x1 = model.getMean(action, x0); - Matrix e1 = g * e0 * gt + r; - - return std::make_shared>(x1, e1); - } - - std::shared_ptr> marginalize( - const ExtendedKalmanActionModel& model, - const GaussianMoment& action, - const GaussianMoment& belief) - { - - X x0 = belief.mean(); - U u = action.mean(); - Matrix e0 = belief.covariance(); - Matrix eu = action.covariance(); - - // time update - Matrix gx = model.getStateJacobian(u, x0); - Matrix gxt = gx.transpose(); - Matrix gu = model.getActionJacobian(action.Mean, x0); - Matrix gut = gu.transpose(); - Matrix r = model.getError(u, x0); - - X x1 = model.getMean(u, x0); - Matrix e1 = gx * e0 * gxt + gu * eu * gut + r; - - return std::make_shared>(x1, e1); - } - - - std::shared_ptr> bayesianInference( - const ExtendedKalmanSensorModel& sensor_model, - const Z& observation, - const GaussianMoment& belief) - { - - // Table 3.3 p59 - X x0 = belief.mean(); - Matrix e0 = belief.covariance(); - - Z expected_z = sensor_model.getMean(x0); - Matrix q = sensor_model.getError(observation); - - Matrix h = sensor_model.getJacobian(x0); - Matrix ht = h.transpose(); - - Matrix i = Matrix::identity(e0.order()); - - Matrix e0_ht = e0 * ht; - Matrix k = e0_ht * (h * e0_ht + q).inverse(); - Vector v = (x0 + k * (observation - expected_z)); - X x1; - x1.set(std::move(v)); - Matrix e1 = (i - k * h) * e0; - - return std::make_shared>(x1, e1); - } - }; - } - } -} - - -#endif // EXTENDED_KALMAN_FILTER_HPP diff --git a/core/navigation/localization/include/localization/information_filter.hpp.hpp b/core/navigation/localization/include/localization/information_filter.hpp.hpp deleted file mode 100644 index 40b749d..0000000 --- a/core/navigation/localization/include/localization/information_filter.hpp.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef INFORMATION_FILTER_HPP -#define INFORMATION_FILTER_HPP - -#include "probability/continuous/gaussian_canonical.hpp" -#include "probability/data/matrix.hpp" -#include "probability/data/vector.hpp" -#include "kalman_filter.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - template - class InformationFilter - { - using probability::GaussianCanonical; - using probability::Matrix; - using probability::Vector; - public: - InformationFilter() {} - virtual ~InformationFilter() {} - - std::shared_ptr> marginalize( - const KalmanActionModel& model, const U& action, - const GaussianCanonical& belief) - { - const Vector& xi = belief.information(); - const Matrix& A = model.a(); - const Matrix& B = model.b(); - const Vector& C = model.c(); - const Matrix& R = model.r(); - Matrix At = A.transpose(); - Matrix Sigma = belief.inversePrecision(); - Matrix ASigma = A * Sigma; - - Matrix nextOmega = (ASigma * At + R).inverse(); - X nextXi; - nextXi.set(nextOmega * (ASigma * xi + B * action + C)); - - return std::make_shared>(std::move(nextXi), std::move(nextOmega)); - } - - std::shared_ptr> bayesianInference( - const KalmanSensorModel& model, - const Z& data, - const GaussianCanonical& belief) - { - - const Vector& Xi = belief.information(); - const Matrix& Omega = belief.precision(); - const Matrix& C = model.c(); - Matrix Ct = C.transpose(); - const Matrix Q = model.q(); - const Matrix QInv = Q.inverse(); - const Matrix CtQInv = Ct * QInv; - - Matrix nextOmega = CtQInv * C + Omega; - Vector nextXi = CtQInv * data + Xi; - - return std::make_shared>(std::move(nextXi), std::move(nextOmega)); - } - }; - - } - } -} - -#endif // INFORMATION_FILTER_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/markov_action_chain.hpp b/core/navigation/localization/include/localization/markov_action_chain.hpp deleted file mode 100644 index a9018cd..0000000 --- a/core/navigation/localization/include/localization/markov_action_chain.hpp +++ /dev/null @@ -1,182 +0,0 @@ -#ifndef MARKOV_ACTION_CHAIN_HPP -#define MARKOV_ACTION_CHAIN_HPP - -#include "localization/probability/discrete/discrete_conditional.hpp" -#include "localization/probability/discrete/distribution_value_map.hpp" - -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - template - class MarkovActionChain : pulbic probability::DiscreteConditional> - { - public: - typedef std::pair XU; - - MarkovActionChain() - { - } - - virtual ~MarkovActionChain() override - { - } - - std::vector domain() const override - { - std::vector result; - result.reserve(m_x.size()); - for (auto& x_x : m_x) - { - result.push_back(x_x.first); - } - return result; - } - - std::vector range() const override - { - std::vector result; - result.reserve(xu_.size()); - for (auto& xu_xu : m_xu) - { - result.push_back(xu_xu.first); - } - return result; - } - - double conditionalProbabilityOf(const X& x, const XU& xu) const override - { - const X& start = xu.first; - const U& u = xu.second; - auto start_action_map = m_p.find(start); - if (start_action_map == m_p.end()) - { - return 0.0; - } - auto action_end_map = start_action_map->second.find(u); - if (action_end_map == start_action_map->second.end()) - { - return 0.0; - } - auto end_value = action_end_map->second.find(x); - if (end_value == action_end_map->second.end()) - { - return 0.0; - } - return end_value->second; - } - - void set(X end, U action, X start, double p) - { - auto start_action_map = m_p.find(start); - if (start_action_map == m_p.end()) - { - start_action_map = m_p.emplace(start, std::map>()).first; - } - auto action_end_map = start_action_map->second.find(action); - if (action_end_map == start_action_map->second.end()) - { - auto emplace_result = start_action_map->second.emplace(action, std::map()); - action_end_map = emplace_result.first; - } - action_end_map->second[end] = p; - m_x[end] = end; - m_u[action] = action; - m_xu[XU(start, action)] = XU(start, action); - } - - double sumCondition(const XU& start_action) const - { - double sum = 0.0; - const X& start = start_action.first; - const U& action = start_action.second; - auto start_action_map = m_p.find(start); - if (start_action_map != m_p.end()) - { - auto action_end_map = start_action_map->second.find(action); - if (action_end_map != start_action_map->second.end()) - { - for (auto& end_value : action_end_map->second) - { - sum += end_value.second; - } - } - } - return sum; - } - - X sampleCondition(const XU& start_action, std::default_random_engine* generator) const - { - double sum = sumLikelihood(start_action); - if (sum == 0.0) - { - throw std::runtime_error("Conditional distribution is not conditioned on start_action. Cannot sample."); - } - std::uniform_real_distribution random(0, sum); - return sampleCondition(start_action, random(*generator)); - } - - X sampleCondition(const XU& start_action, double p) const - { - const X& start = start_action.first; - const U& action = start_action.second; - auto start_action_map = m_p.find(start); - if (start_action_map == m_p.end()) - { - throw std::runtime_error("MarkovActionChain does not have start state. Cannot sample."); - } - auto action_end_map = start_action_map->second.find(action); - if (action_end_map == start_action_map->second.end()) - { - throw std::runtime_error("MarkovActionChain does not have action from start state. Cannot sample."); - } - for (auto& end_value : action_end_map->second) - { - p -= end_value.second; - if (p <= 0) - { - return end_value.first; - } - } - throw std::runtime_error("End state for specified is badly specified. Cannot sample."); - } - - std::shared_ptr> marginalize(const probability::RandomDistribution& start_action) - { - auto result = std::make_shared > (); - for (auto& level0 : m_p) - { - X start = level0.first; - for (auto& level1 : level0.second) - { - U action = level1.first; - for (auto& level2 : level1.second) - { - X end = level2.first; - double p0 = level2.second; - XU xu(start, action); - double p1 = start_action.probabilityOf(xu); - double current_end_p = result->probabilityOf(end); - result->set(end, p0 * p1 + current_end_p); - } - } - } - result->normalize(); - return result; - } - - private: - std::map>> m_p; - std::map m_x; - std::map m_u; - std::map m_xu; - }; - } - } -} - -#endif // MARKOV_ACTION_CHAIN_HPP diff --git a/core/navigation/localization/include/localization/probability/bayes_distribution.hpp b/core/navigation/localization/include/localization/probability/bayes_distribution.hpp deleted file mode 100644 index 2194460..0000000 --- a/core/navigation/localization/include/localization/probability/bayes_distribution.hpp +++ /dev/null @@ -1,95 +0,0 @@ -#ifndef BAYES_DISTRIBUTION_HPP -#define BAYES_DISTRIBUTION_HPP - -#include -#include - -#include "random_distribution.hpp" -#include "random_conditional.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class BayesDistribution : public RandomDistribution, - public std::enable_shared_from_this> - { - public: - BayesDistribution(std::shared_ptr> prior, - std::shared_ptr> sensor_model, - Y observation) - : m_prior(prior) - , m_sensor_model(sensor_model) - , m_data(std::move(observation)) - {} - - virtual ~BayesDistribution() {} - - double probabilityOf(const X& x) const override - { - double pyx = m_sensor_model->conditionalProbabilityOf(m_data, x); - double px = m_prior->probabilityOf(x); - double py = m_normalizer; - return pyx * px / py; - } - - X sample(std::default_random_engine*) const override - { - throw std::runtime_error("Not Implemented"); - } - - std::shared_ptr> chainObservation(Y y) - { - return std::make_shared< BayesDistribution >(this->shared_from_this(), m_sensor_model, std::move(y)); - } - - double estimateNormalizer(const std::vector& domain, double dx) - { - double sum = 0.0; - for (const X& x : domain) - { - double pz_given_x = m_sensor_model->conditionalProbabilityOf(m_data, x); - double px = m_prior->probabilityOf(x); - sum += pz_given_x * px; - } - m_normalizer = sum * dx; - return m_normalizer; - } - - const RandomConditional& sensorModel() const - { - return *m_sensor_model; - } - - const Y& data() const - { - return m_data; - } - - double& normalizer() - { - return m_normalizer; - } - - double normalizer() const - { - return m_normalizer; - } - - private: - std::shared_ptr> m_prior; - std::shared_ptr> m_sensor_model; - Y m_data; - double m_normalizer = 1.0; - }; - } - } - } -} - -#endif // BAYES_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/probability/conditional_map.hpp b/core/navigation/localization/include/localization/probability/conditional_map.hpp deleted file mode 100644 index 36fa096..0000000 --- a/core/navigation/localization/include/localization/probability/conditional_map.hpp +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef CONDITIONAL_MAP_HPP -#define CONDITIONAL_MAP_HPP - -#include -#include - -#include "random_distribution.hpp" -#include "random_conditional.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class ConditionalMap : public RandomConditional - { - public: - ConditionalMap() - {} - - virtual ~ConditionalMap() - {} - - void set(Y y, std::shared_ptr> x) - { - m_map[y] = x; - } - - double conditionalProbabilityOf(const X& x, const Y& y) const override - { - double result = 0.0; - auto key_value = m_map.find(y); - if (key_value != m_map.end()) - { - result = key_value->second->probabilityOf(x); - } - return result; - } - - private: - std::map>> m_map; - }; - } - } - } -} - -#endif // CONDITIONAL_MAP_HPP diff --git a/core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp b/core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp deleted file mode 100644 index 4cd05f6..0000000 --- a/core/navigation/localization/include/localization/probability/continuous/gaussian_canonical.hpp +++ /dev/null @@ -1,134 +0,0 @@ -#ifndef GAUSSIAN_CANONICAL_HPP -#define GAUSSIAN_CANONICAL_HPP - -#include -#include - -#include "../random_distribution.hpp" -#include "../data/vector.hpp" -#include "../data/matrix.hpp" -#include "gaussian_moment.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class GaussianCanonical : public RandomDistribution - { - public: - GaussianCanonical(const Vector& xi, const Matrix& omega) : - m_information(std::move(xi)), - m_precision(std::move(omega)), - m_sigma(omega.rows(), omega.cols()), - m_mu() - { - } - - explicit GaussianCanonical(const GaussianMoment& x) : - m_information(x.covariance().Inverse()), - m_precision(m_information * x.mean()), - m_mu(x.mean()), - m_sigma(x.covariance()), - m_is_sigma_valid(true), - m_is_mu_valid(true) - { - } - GaussianCanonical(GaussianCanonical&& that) : - m_precision(std::move(that.m_precision)), - m_information(std::move(that.m_information)), - m_is_sigma_valid(that.m_is_sigma_valid), - m_is_mu_valid(that.m_is_mu_valid), - m_sigma(std::move(that.m_sigma)), - m_mu(std::move(that.m_mu)) - { - } - - GaussianCanonical& operator=(GaussianCanonical&& that) - { - this->m_precision = std::move(that.m_precision); - this->m_information = std::move(that.m_information); - this->m_is_sigma_valid = that.m_is_sigma_valid; - this->m_is_mu_valid = that.m_is_mu_valid; - this->m_sigma = std::move(that.m_sigma); - this->m_mu = std::move(that.m_mu); - return *this; - } - - virtual ~GaussianCanonical() override - { - } - - const Vector& information() const - { - return m_information; - } - const Matrix& precision() const - { - return m_precision; - } - const Matrix& sigma() const - { - return inversePrecision(); - } - const Vector& mu() const - { - return getMu(); - } - - const Matrix& inversePrecision() const - { - if (!m_is_sigma_valid) - { - m_sigma = precision().inverse(); - m_is_sigma_valid = true; - } - return m_sigma; - } - - const X& getMu() const - { - if (!m_is_mu_valid) - { - m_mu.set(sigma() * information()); - m_is_mu_valid = true; - } - return m_mu; - } - - double probabilityOf(const X&) const override - { - throw std::runtime_error("GaussianCanonical::ProbabilityOf not implemented."); - } - - X sample(std::default_random_engine*) const override - { - throw std::runtime_error("GaussianCanonical::Sample not implemented."); - } - - private: - Vector m_information; // Omega - Matrix m_precision; // xi - mutable bool m_is_sigma_valid = false; - mutable bool m_is_mu_valid = false; - mutable Matrix m_sigma; - mutable X m_mu; - - }; - - template - static std::ostream& operator<<(std::ostream& os, const GaussianCanonical& x) - { - os << "omega=" << x.information() << ", xi=" << x.precision(); - return os; - }; - } - } - } -} - -#endif // GAUSSIAN_CANONICAL_HPP diff --git a/core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp b/core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp deleted file mode 100644 index f367e96..0000000 --- a/core/navigation/localization/include/localization/probability/continuous/gaussian_moment.hpp +++ /dev/null @@ -1,132 +0,0 @@ -#ifndef GAUSSIAN_MOMENT_HPP -#define GAUSSIAN_MOMENT_HPP - -#include -#include -#include - -#include "../random_distribution.hpp" -#include "../data/vector.hpp" -#include "../data/matrix.hpp" -#include "../data/matrix_vector_operators.hpp" - - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class GaussianMoment : public RandomDistribution - { - public: - static double fn(double x, double mean, double variance) - { - double A = sqrt(1.0 / (2 * M_PI * variance)); - double dx = x - mean; - double x2 = dx * dx; - double e = exp(-0.5 * x2 / variance); - return A * e; - } - static double fn(const Vector& x, const Vector& mean, const Matrix& covariance, const Matrix& inverse_covariance) - { - double A = sqrt(1.0 / ((2 * M_PI) * covariance).determinant()); - Vector dx = x - mean; - double e = exp(-(dx * inverse_covariance * dx)); - return A * e; - } - static double fn(const Vector& x, const Vector& mean, const Matrix& covariance) - { - return fn(x, mean, covariance, covariance.inverse()); - } - - GaussianMoment(X mean, Matrix covariance) - : m_mean(std::move(mean)) - , m_covariance(std::move(covariance)) - { - } - - GaussianMoment(GaussianMoment&& that) - : m_mean(std::move(that.m_mean)) - , m_covariance(std::move(that.m_covariance)) - { - } - - GaussianMoment& operator=(GaussianMoment&& that) { - this->m_mean = std::move(that.m_mean); - this->m_covariance = std::move(that.m_covariance); - this->m_inverse_covariance = std::move(that.m_inverse_covariance); - this->m_sqrt_covariance = std::move(that.m_sqrt_covariance); - this->m_is_valid_inverse = m_is_valid_inverse; - this->m_is_valid_sqrt = m_is_valid_sqrt; - return *this; - } - - virtual ~GaussianMoment() - { - } - - double probabilityOf(const X& x) const override - { - return fn(x.aliasVector(), mean().aliasVector(), covariance(), inverseCovariance()); - } - - X sample(std::default_random_engine* generator) const override { - std::normal_distribution n01(0, 1); - Vector sample(order()); - for (std::size_t i = 0; i < sample.order(); i++) { - // need a N(0,1) here - sample[i] = n01(*generator); - } - X result; - result.set(mean() + sqrtCovariance() * sample); - return result; - } - - std::size_t order() const { - return mean().order(); - } - - const X& mean() const { - return m_mean; - } - - const Matrix& covariance() const { - return m_covariance; - } - - const Matrix& inverseCovariance() const { - if (!m_is_valid_inverse) { - m_inverse_covariance = m_covariance.inverse(); - m_is_valid_inverse = true; - } - return m_inverse_covariance; - } - - const Matrix& sqrtCovariance() const { - if (!m_is_valid_sqrt) { - m_sqrt_covariance = covariance().sqrt(); - m_is_valid_sqrt = true; - } - return m_sqrt_covariance; - } - - private: - X m_mean; - Matrix m_covariance; - // These values are mutable because they are cached derivations of covariance - mutable Matrix m_inverse_covariance; - mutable Matrix m_sqrt_covariance; - mutable bool m_is_valid_inverse = false; - mutable bool m_is_valid_sqrt = false; - }; - } - } - } -} - - -#endif // GAUSSIAN_MOMENT_HPP diff --git a/core/navigation/localization/include/localization/probability/data/matrix.hpp b/core/navigation/localization/include/localization/probability/data/matrix.hpp deleted file mode 100644 index 5ca9058..0000000 --- a/core/navigation/localization/include/localization/probability/data/matrix.hpp +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef MATRIX_HPP -#define MATRIX_HPP - -#include -#include -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - - class Vector; - - class Matrix - { - public: - static Matrix identity(size_t order) - { - std::vector a(order * order); - for (std::size_t i = 0; i < a.size(); i += order + 1) - { - a[i] = 1.0; - } - return Matrix(order, order, a); - } - - Matrix(); - Matrix(std::size_t rows, std::size_t cols); - Matrix(std::size_t rows, std::size_t cols, std::vector values); - Matrix(std::size_t rows, std::size_t cols, std::vector values, bool row_based); - Matrix(const Matrix& that); - Matrix(Matrix&& that); - - virtual ~Matrix(); - - std::size_t rows() const; - std::size_t cols() const; - std::size_t order() const; - double at(std::size_t row, std::size_t col) const; - Vector column(std::size_t col) const; - Matrix& operator=(Matrix&& that); - Matrix& operator+=(const Matrix& that); - Matrix operator+(const Matrix& that) const; - Matrix operator-(const Matrix& that) const; - Matrix operator*(const Matrix& that) const; - Matrix transpose() const; - Matrix inverse() const; - Matrix sqrt() const; - double determinant() const; - void scaleRow(std::size_t r, double k); - void combineRow(std::size_t srcRow, std::size_t dstRow, double k); - Matrix luDecompositionByGE(int* p_swap_count, std::vector* p_pivot) const; - Matrix roundSymmetry(); - - private: - std::size_t getIndex(std::size_t row, std::size_t col) const; - static void initPivots(std::size_t n, std::vector* p_pivot); - double partialPivot(std::vector* p_pivot, std::size_t k, int* p_swap_count) const; - bool isSymmetric() const; - Matrix choleskyDecomposition() const; - std::size_t m_rows; - std::size_t m_cols; - std::vector m_m; - bool m_row_based = true; - - friend Matrix operator*(double scalar, const Matrix& m); - }; - - std::ostream& operator<<(std::ostream& os, const Matrix& v); - } - } - } -} - -#endif // MATRIX_HPP diff --git a/core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp b/core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp deleted file mode 100644 index 04d10c7..0000000 --- a/core/navigation/localization/include/localization/probability/data/matrix_vector_operators.hpp +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef MATRIX_VECTOR_OPERATORS_HPP -#define MATRIX_VECTOR_OPERATORS_HPP - -#include "localization/probability/data/matrix.hpp" -#include "localization/probability/data/vector.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - Vector operator*(const Matrix& m, const Vector& v); - Vector operator*(const Vector& v, const Matrix& m); - Matrix operator*(double scalar, const Matrix& m); - double operator*(const Vector& v1, const Vector& v2); - } - } - } -} - - -#endif // MATRIX_VECTOR_OPERATORS_HPP diff --git a/core/navigation/localization/include/localization/probability/data/vector.hpp b/core/navigation/localization/include/localization/probability/data/vector.hpp deleted file mode 100644 index 46ad24a..0000000 --- a/core/navigation/localization/include/localization/probability/data/vector.hpp +++ /dev/null @@ -1,73 +0,0 @@ -#ifndef VECTOR_HPP -#define VECTOR_HPP - -#include -#include -#include -#include -#include -#include -#include - -#include "localization/probability/data/matrix.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - - class Vector - { - public: - Vector(); - explicit Vector(std::vector a); - explicit Vector(const Vector& v); - Vector(Vector&& v); - explicit Vector(std::size_t order); - virtual ~Vector(); - std::size_t order() const; - double& at(int i); - double at(int i) const; - double& operator[](std::size_t i); - double operator[](std::size_t i) const; - Matrix cross(const Vector& that) const; - Vector& operator=(const Vector& that); - virtual void set(Vector a); - void copyAssign(const Vector& v); - void moveAssign(Vector&& v); - Vector aliasVector() const; - double dot(const Vector& v2) const; - Vector operator+(const Vector& v2) const; - Vector operator-(const Vector& v2) const; - bool operator<(const Vector& v2) const; - bool operator>(const Vector& v2) const; - bool operator==(const Vector& v) const; - protected: - - private: - std::vector m_a; - }; - - std::ostream& operator<<(std::ostream& os, const Vector& v); - - -#define DEFINE_VECTOR1(X) \ -class X : public ::rip::navigation::localization::probability::Vector { \ -public: \ - X() : Vector(std::vector({ 0.0 })) {} \ - X(double x) : Vector(std::vector({ x })) {} \ - X(const X& v) : Vector(v) {} \ - X(const X&& v) : Vector(v) {} \ - X(::rip::navigation::localization::probability::Vector&& v) : Vector(v) {} \ - X& operator=(const X& that) { Vector::operator=(that); return *this; } \ -} - } - } - } -} - -#endif // VECTOR_HPP diff --git a/core/navigation/localization/include/localization/probability/data/vector_range.hpp b/core/navigation/localization/include/localization/probability/data/vector_range.hpp deleted file mode 100644 index 0fd49b9..0000000 --- a/core/navigation/localization/include/localization/probability/data/vector_range.hpp +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef VECTOR_RANGE_HPP -#define VECTOR_RANGE_HPP - -#include -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class VectorRange - { - public: - VectorRange(X min, X max) - : m_min(std::move(min)), m_max(std::move(max)) - { - } - virtual ~VectorRange() - { - } - - double area() const - { - Vector dx = m_max - m_min; - double result = 1.0; - for (unsigned int i = 0; i < dx.order(); i++) - { - result *= dx[i]; - } - return fabs(result); - } - - bool contains(const X& x) const - { - return x > m_min && x < m_max; - } - - X sample(std::default_random_engine* generator) const - { - X result; - for (unsigned int i = 0; i < result.order(); i++) - { - std::uniform_real_distribution random(m_min[i], m_max[i]); - result[i] = random(*generator); - } - return result; - } - - private: - X m_min; - X m_max; - }; - } - } - } -} - -#endif // VECTOR_RANGE_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp b/core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp deleted file mode 100644 index 5af366a..0000000 --- a/core/navigation/localization/include/localization/probability/discrete/conditional_value_map.hpp +++ /dev/null @@ -1,170 +0,0 @@ -#ifndef CONDITIONAL_VALUE_MAP_HPP -#define CONDITIONAL_VALUE_MAP_HPP - -#include -#include -#include -#include -#include - -#include "discrete_conditional.hpp" -#include "distribution_value_map.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class ConditionalValueMap : public DiscreteConditional - { - public: - ConditionalValueMap() {} - - virtual ~ConditionalValueMap() override {} - - virtual std::vector domain() const override - { - std::vector keys; - std::transform( - m_domain.begin(), - m_domain.end(), - std::back_inserter(keys), - [](const std::pair& pair){return pair.first;}); - return keys; - /* - std::vector result; - result.reserve(m_domain.size()); - for (auto& pair : m_domain) - { - result.push_back(pair.first); - } - return result; - */ - } - - virtual std::vector range() const override - { - std::vector result; - result.reserve(m_map.size()); - for (auto& pair : m_map) - { - result.push_back(pair.first); - } - return result; - } - - double conditionalProbabilityOf(const X& x, const Y& y) const override - { - auto y_map = m_map.find(y); - if (y_map == m_map.end()) - { - // todo: Does zero make sense here? Does an unknown imply zero change - return 0.0; - } - - auto x_value = y_map->second.find(x); - if (x_value == y_map->second.end()) - { - return 0.0; - } - return x_value->second; - } - - void set(const X& x, const Y& y, double p) - { - auto value_map = m_map.find(y); - if (value_map == m_map.end()) - { - value_map = m_map.emplace(y, std::map()).first; - } - value_map->second[x] = p; - m_domain[x] = x; - } - - std::shared_ptr> likelihoodOf(const X& data) const - { - auto result = std::make_shared>(); - for (auto& y_map : m_map) - { - auto x_value = y_map.second.find(data); - if (x_value != y_map.second.end()) - { - result->set(y_map.first, x_value->second); - } - } - return result; - } - - std::shared_ptr> bayesianInference(X data, const DistributionValueMap& prior) const - { - auto likelihood = likelihoodOf(data); - return likelihood->product(prior); - } - - double sumLikelihood(const X& x) const - { - double sum = 0.0; - for (auto& y_map : m_map) - { - auto x_value = y_map.second.find(x); - if (x_value != y_map.second.end()) - { - sum += x_value->second; - } - } - return sum; - } - - Y sampleLikelihood(const X& x, double p) const - { - for (auto& y_map : m_map) - { - Y y = y_map.first; - auto x_value = y_map.second.find(x); - if (x_value != y_map.second.end()) - { - p -= x_value->second; - if (p < 0) - { - return y; - } - } - } - throw std::runtime_error("Conditional distribution is not conditioned on x. Cannot sample."); - } - - template - std::shared_ptr> marginalize(const DiscreteConditional& that) - { - auto result = std::make_shared>(); - for (X& x : this->domain()) - { - for (Z& z : that.range()) - { - double p = 0.0; - for (Y& y : that.domain()) - { - double xy = this->conditionalProbabilityOf(x, y); - double yz = that.conditionalProbabilityOf(y, z); - p += xy * yz; - } - result->set(x, z, p); - } - } - return result; - } - - private: - std::map> m_map; - std::map m_domain; - }; - } - } - } -} - -#endif // CONDITIONAL_VALUE_MAP_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp b/core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp deleted file mode 100644 index ced4b58..0000000 --- a/core/navigation/localization/include/localization/probability/discrete/discrete_conditional.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef DISCRETE_CONDITIONAL_HPP -#define DISCRETE_CONDITIONAL_HPP - -#include -#include - -#include "../random_conditional.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - - template - class DiscreteConditional : public RandomConditional - { - public: - DiscreteConditional() {} - - virtual ~DiscreteConditional() override {} - - virtual std::vector domain() const = 0; - - virtual std::vector range() const = 0; - }; - - } - } - } -} - -#endif // DISCRETE_CONDITIONAL_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp b/core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp deleted file mode 100644 index b6e7b33..0000000 --- a/core/navigation/localization/include/localization/probability/discrete/discrete_distribution.hpp +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef DISCRETE_DISTRIBUTION_HPP -#define DISCRETE_DISTRIBUTION_HPP - -#include - -#include "../random_distribution.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class DiscreteDistribution : public RandomDistribution - { - public: - DiscreteDistribution() {} - - virtual ~DiscreteDistribution() {} - - protected: - template - static X selectFromMap(const std::map& map, double select) - { - if (map.empty()) - { - throw std::runtime_error("Cannot sample from an empty distribution"); - } - X result; - auto p = map.begin(); - do - { - result = p->first; - select -= p->second; - } - while (++p != map.end() && select >= 0); - return result; - } - }; - } - } - } -} - -#endif // DISCRETE_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp b/core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp deleted file mode 100644 index 7438c84..0000000 --- a/core/navigation/localization/include/localization/probability/discrete/distribution_counter.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#ifndef DISTRIBUTION_COUNTER_HPP -#define DISTRIBUTION_COUNTER_HPP - -#include -#include - -#include "discrete_distribution.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class DistributionCounter : public DiscreteDistribution - { - public: - DistributionCounter() {} - - virtual ~DistributionCounter() {} - - double probabilityOf(const X& x) const override - { - double result = 0.0; - auto key_value = m_map.find(x); - if (key_value != m_map.end()) - { - double n = key_value->second; - result = n / m_total; - } - return result; - } - - X sample(std::default_random_engine* generator) const override - { - std::uniform_real_distribution random(0, m_total); - double select = random(*generator); - return this->selectFromMap(m_map, select); - } - - void addObservation(X x) - { - int n = 0; - auto key_value = m_map.find(x); - if (key_value != m_map.end()) - { - n = key_value->second; - } - m_map[x] = n + 1; - m_total ++; - } - - private: - int m_total = 0; - std::map m_map; - }; - } - } - } -} - -#endif // DISTRIBUTION_COUNTER_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp b/core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp deleted file mode 100644 index ecc1b37..0000000 --- a/core/navigation/localization/include/localization/probability/discrete/distribution_value_map.hpp +++ /dev/null @@ -1,118 +0,0 @@ -#ifndef DISTRIBUTION_VALUE_MAP_HPP -#define DISTRIBUTION_VALUE_MAP_HPP - -#include -#include -#include -#include - -#include "discrete_distribution.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class DistributionValueMap : public DiscreteDistribution - { - public: - DistributionValueMap() {} - - DistributionValueMap(X x0, double p0, X x1, double p1) - { - set(x0, p0); - set(x1, p1); - normalize(); - } - - DistributionValueMap(const std::vector& list) - { - for (auto& x : list) - { - set(x, 1.0); - } - normalize(); - } - - virtual ~DistributionValueMap() override {} - - double probabilityOf(const X& x) const override - { - double result = 0.0; - auto it = m_map.find(x); - if (it != m_map.end()) - { - result = it->second; - } - return result; - } - - X sample(std::default_random_engine* generator) const override - { - std::uniform_real_distribution random(0, 1); - double select = random(*generator); - return this->selectFromMap(m_map, select); - } - - void set(X x, double p) - { - m_map[x] = p; - } - - void normalize() - { - double sum = 0.0; - for (auto& pair : m_map) - { - sum += pair.second; - } - for (auto& pair : m_map) - { - pair.second /= sum; - } - } - - double entropy() const - { - double result = 0.0; - for (auto& pair : m_map) - { - double p = pair.second; - if (p != 0) - { - result -= p * log2(p); - } - } - return result; - } - - std::shared_ptr> product(const DistributionValueMap& that) const - { - auto result = std::make_shared>(); - for (auto& pair : m_map) - { - double lp = pair.second; - double rp = that.probabilityOf(pair.first); - double p = lp * rp; - if (p != 0) - { - result->set(pair.first, p); - } - } - result->normalize(); - return result; - } - - private: - std::map m_map; - }; - } - } - } -} - -#endif // DISTRIBUTION_VALUE_MAP_HPP diff --git a/core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp b/core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp deleted file mode 100644 index e003718..0000000 --- a/core/navigation/localization/include/localization/probability/discrete/markov_chain.hpp +++ /dev/null @@ -1,158 +0,0 @@ -#ifndef MARKOV_CHAIN_HPP -#define MARKOV_CHAIN_HPP - -#include -#include - -#include "discrete_conditional.hpp" -#include "distribution_value_map.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class MarkovChain : public DiscreteConditional - { - public: - MarkovChain() {} - - virtual ~MarkovChain() override {} - - std::vector domain() const override - { - std::vector result; - result.reserve(m_range.size()); - for (auto& pair : m_range) - { - result.push_back(pair.first); - } - return result; - } - - std::vector range() const override - { - std::vector result; - result.reserve(m_transitions.size()); - for (auto& pair : m_transitions) - { - result.push_back(pair.first); - } - return result; - } - - double conditionalProbabilityOf(const X& end, const X& start) const override - { - auto end_map = m_transitions.find(start); - if (end_map == m_transitions.end()) - { - // todo: Does zero make sense here? Does an unknown imply zero chance? - return 0.0; - } - auto result = end_map->second.find(end); - if (result == end_map->second.end()) - { - return 0.0; - } - return result->second; - } - - void set(const X& end, const X& start, double p) - { - auto end_map = m_transitions.find(start); - if (end_map == m_transitions.end()) - { - end_map = m_transitions.emplace(start, std::map()).first; - } - end_map->second[end] = p; - m_range[end] = end; - } - - X doTransition(const X& start, std::default_random_engine* generator) - { - std::uniform_real_distribution dist(0, 1); - double select = dist(*generator); - X result = start; - auto end_map = m_transitions.find(start); - if (end_map != m_transitions.end()) - { - for (auto& x : end_map->second) - { - select -= x.second; - if (select <= 0) - { - result = x.first; - break; - } - } - } - return result; - } - - std::shared_ptr reverse() const - { - auto result = std::make_shared>(); - for (X today : range()) - { - auto x = this->bayesianInference(today, DistributionValueMap(domain())); - for (X yesterday : domain()) - { - double p = x->probabilityOf(yesterday); - result->set(yesterday, today, p); - } - } - return result; - } - - std::shared_ptr> likelihoodOf(const X& data) const - { - auto result = std::make_shared>(); - for (auto& end_map : m_transitions) - { - auto end = end_map.second.find(data); - if (end != end_map.second.end()) - { - result->set(end_map.first, end->second); - } - } - return result; - } - - std::shared_ptr> marginalize(const RandomDistribution& start) const - { - auto result = std::make_shared>(); - for (auto& xfr_map : m_transitions) - { - double pstart = start.probabilityOf(xfr_map.first); - for (auto& end : xfr_map.second) - { - double pend = end.second; - double p0 = result->probabilityOf(end.first); - double pxfr = pend * pstart; - double p1 = p0 + pxfr; - result->set(end.first, p1); - } - } - result->normalize(); - return result; - } - - std::shared_ptr> bayesianInference(const X& data, const DistributionValueMap& prior) const - { - auto likelihood = likelihoodOf(data); - return likelihood->product(prior); - } - private: - std::map> m_transitions; - std::map m_range; - }; - } - } - } -} - -#endif // MARKOV_CHAIN_HPP diff --git a/core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp b/core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp deleted file mode 100644 index 042ab78..0000000 --- a/core/navigation/localization/include/localization/probability/independent_pair_distribution.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef INDEPENDENT_PAIR_DISTRIBUTION_HPP -#define INDEPENDENT_PAIR_DISTRIBUTION_HPP - -#include -#include - -#include "random_distribution.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class IndependentPairDistribution : public RandomDistribution> - { - public: - typedef std::pair AB; - - IndependentPairDistribution(std::shared_ptr< RandomDistribution > a, std::shared_ptr< RandomDistribution > b) - : m_a(a) - , m_b(b) - { - - } - - virtual ~IndependentPairDistribution() - { - } - - double probabilityOf(const AB& t) const override - { - return m_a->probabilityOf(t.first) * m_b->probabilityOf(t.second); - } - - AB sample(std::default_random_engine* generator) const override - { - return AB(m_a->sample(generator), m_b->sample(generator)); - } - - private: - std::shared_ptr< RandomDistribution > m_a; - std::shared_ptr< RandomDistribution > m_b; - - }; - - } - } - } -} -#endif // INDEPENDENT_PAIR_DISTRIBUTION_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/probability/random_conditional.hpp b/core/navigation/localization/include/localization/probability/random_conditional.hpp deleted file mode 100644 index c179a57..0000000 --- a/core/navigation/localization/include/localization/probability/random_conditional.hpp +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef RANDOM_CONDITIONAL_HPP -#define RANDOM_CONDITIONAL_HPP - -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class RandomConditional - { - public: - RandomConditional() {} - - virtual ~RandomConditional() {} - - virtual double conditionalProbabilityOf(const X& x, const Y& y) const = 0; - }; - } - } - } -} - -#endif // RANDOM_CONDITIONAL_HPP diff --git a/core/navigation/localization/include/localization/probability/random_distribution.hpp b/core/navigation/localization/include/localization/probability/random_distribution.hpp deleted file mode 100644 index 37588ac..0000000 --- a/core/navigation/localization/include/localization/probability/random_distribution.hpp +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef RANDOM_DISTRIBUTION_HPP -#define RANDOM_DISTRIBUTION_HPP - -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class RandomDistribution - { - public: - RandomDistribution() {} - - virtual ~RandomDistribution() {} - - virtual double probabilityOf(const X& x) const = 0; - - virtual X sample(std::default_random_engine* generator) const = 0; - }; - } - } - } -} - -#endif // RANDOM_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/probability/random_uniform.hpp b/core/navigation/localization/include/localization/probability/random_uniform.hpp deleted file mode 100644 index fa5fd4e..0000000 --- a/core/navigation/localization/include/localization/probability/random_uniform.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef RANDOM_UNIFORM_HPP -#define RANDOM_UNIFORM_HPP - -#include "data/vector.hpp" -#include "data/vector_range.hpp" -#include "random_distribution.hpp" - -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class RangedUniform : public RandomDistribution - { - public: - RangedUniform(X min, X max) - : m_range(std::move(min), std; : move(max)) - { - static_assert(std::is_base_of::value, "X must derive from Vector"); - } - - ~RangedUniform() override {} - - double probabilityOf(const X& x) const override - { - if (m_range.contains(x)) - { - - return 1.0 / m_range.area(); - } - return 0.0; - } - - X sample(std::default_random_engine* generator) const override - { - return m_range.sample(generator); - } - - private: - VectorRange m_range; - }; - } - } - } -} - -#endif // RANDOM_UNIFORM_HPP diff --git a/core/navigation/localization/include/localization/probability/ranged_uniform.hpp b/core/navigation/localization/include/localization/probability/ranged_uniform.hpp deleted file mode 100644 index ec80794..0000000 --- a/core/navigation/localization/include/localization/probability/ranged_uniform.hpp +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef RANGED_UNIFORM_HPP -#define RANGED_UNIFORM_HPP - -#include "localization/probability/data/vector.hpp" -#include "localization/probability/data/vector_range.hpp" -#include "random_distribution.hpp" - -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - template - class RangedUniform : public RandomDistribution { - public: - RangedUniform(X min, X max) - : m_range(std::move(min), std::move(max)) - { - static_assert(std::is_base_of::value, "X must derive from Vector"); - } - - ~RangedUniform() override { - } - - double probabilityOf(const X& x) const override { - if (m_range.contains(x)) - { - return 1.0 / m_range.area(); - } - return 0.0; - } - - X sample(std::default_random_engine* generator) const override - { - return m_range.sample(generator); - } - - private: - VectorRange m_range; - }; - } - } - } -} -#endif // RANGED_UNIFORM_HPP \ No newline at end of file diff --git a/core/navigation/localization/src/probability/data/matrix.cpp b/core/navigation/localization/src/probability/data/matrix.cpp deleted file mode 100644 index 2157228..0000000 --- a/core/navigation/localization/src/probability/data/matrix.cpp +++ /dev/null @@ -1,524 +0,0 @@ -#include "localization/probability/data/vector.hpp" -#include "localization/probability/data/matrix.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - Matrix::Matrix() - : m_rows(0) - , m_cols(0) - , m_m() - , m_row_based(true) - { - } - - Matrix::Matrix(std::size_t rows, std::size_t cols) - : m_rows(rows) - , m_cols(cols) - , m_m(rows * cols) - , m_row_based(true) - { - } - - Matrix::Matrix(std::size_t rows, std::size_t cols, std::vector values) - : m_rows(rows) - , m_cols(cols) - , m_m(values) - , m_row_based(true) - { - } - - Matrix::Matrix(std::size_t rows, std::size_t cols, std::vector values, bool row_based) - : m_rows(rows) - , m_cols(cols) - , m_m(values) - , m_row_based(row_based) - { - } - - Matrix::Matrix(const Matrix& that) - : m_rows(that.m_rows) - , m_cols(that.m_cols) - , m_m(that.m_m) - , m_row_based(that.m_row_based) - { - } - - Matrix::Matrix(Matrix&& that) - : m_rows(that.m_rows) - , m_cols(that.m_cols) - , m_m(std::move(that.m_m)) - , m_row_based(that.m_row_based) - { - } - - Matrix::~Matrix() - { - } - - std::size_t Matrix::rows() const - { - return m_rows; - } - - std::size_t Matrix::cols() const - { - return m_cols; - } - - std::size_t Matrix::order() const - { - if (m_rows != m_cols) - { - throw std::runtime_error("Matrix must be square."); - } - return m_rows; - } - - double Matrix::at(std::size_t row, std::size_t col) const - { - return m_m[getIndex(row, col)]; - } - - Vector Matrix::column(std::size_t col) const - { - Vector result; - std::vector a(m_rows); - if (m_row_based) - { - for (std::size_t i = 0; i < a.size(); i++) - { - a[i] = at(i, col); - } - } - else - { - std::size_t col_start = getIndex(0, col); - std::size_t col_end = col_start + a.size(); - std::copy(m_m.begin() + col_start, m_m.begin() + col_end, a.begin()); - } - return Vector(a); - } - - Matrix& Matrix::operator=(Matrix&& that) - { - m_rows = that.m_rows; - m_cols = that.m_cols; - m_m = std::move(that.m_m); - m_row_based = that.m_row_based; - return *this; - } - - Matrix& Matrix::operator+=(const Matrix& that) - { - if (m_rows != that.m_rows || m_cols != that.m_cols) - { - throw std::runtime_error("Matrix sizes not compatible."); - } - - if (m_row_based == that.m_row_based) - { - for (std::size_t i = 0; i < m_m.size(); i++) - { - m_m[i] += that.m_m[i]; - } - } - else - { - for (std::size_t row = 0; row < rows(); row++) - { - for (std::size_t col = 0; col < cols(); col++) - { - std::size_t this_index = getIndex(row, col); - std::size_t that_index = that.getIndex(row, col); - m_m[this_index] += that.m_m[that_index]; - } - } - } - - return *this; - } - - Matrix Matrix::operator+(const Matrix& that) const - { - if (m_rows != that.m_rows || m_cols != that.m_cols || m_row_based != that.m_row_based) - { - throw std::runtime_error("Matrix sizes not compatible."); - } - std::vector m(m_m.size()); - for (std::size_t i = 0; i < m_m.size(); i++) - { - m[i] = m_m[i] + that.m_m[i]; - } - return Matrix(m_rows, m_cols, m, m_row_based); - } - - Matrix Matrix::operator-(const Matrix& that) const - { - if (m_rows != that.m_rows || m_cols != that.m_cols || m_row_based != that.m_row_based) - { - throw std::runtime_error("Matrix sizes not compatible."); - } - std::vector m(m_m.size()); - for (std::size_t i = 0; i < m_m.size(); i++) - { - m[i] = m_m[i] - that.m_m[i]; - } - return Matrix(m_rows, m_cols, m, m_row_based); - } - - Matrix Matrix::operator*(const Matrix& that) const - { - if (m_cols != that.m_rows) - { - throw std::runtime_error("Matrix sizes not compatible."); - } - Matrix result(m_rows, that.m_cols); - for (std::size_t col = 0; col < that.m_cols; col++) - { - for (std::size_t row = 0; row < m_rows; row++) - { - for (std::size_t k = 0; k < m_cols; k++) - { - result.m_m[result.getIndex(row, col)] += at(row, k) * that.at(k, col); - } - } - } - return result; - } - - Matrix Matrix::transpose() const - { - return Matrix(m_cols, m_rows, m_m, !m_row_based); - } - - Matrix Matrix::inverse() const - { - if (m_rows != m_cols) - { - throw std::runtime_error("Matrix must be square."); - } - // set up mean as identity - Matrix result(m_rows, m_cols); - for (std::size_t i = 0; i < m_cols; i++) - { - result.m_m[result.getIndex(i, i)] = 1.0; - } - - Matrix working(m_rows, m_cols, m_m); // creates a copy of m_ - // Eliminate L - for (std::size_t col = 0; col < m_cols; col++) - { - double diag = working.at(col, col); - for (std::size_t row = col + 1; row < m_rows; row++) - { - double target = working.at(row, col); - double a = -target / diag; - working.combineRow(col, row, a); - result.combineRow(col, row, a); - } - working.scaleRow(col, 1.0 / diag); - result.scaleRow(col, 1.0 / diag); - } - // Eliminate U - for (std::size_t col = m_cols - 1; col >= 1; col--) - { - double diag = working.at(col, col); // 1.0 - for (std::size_t row_plus_one = col; row_plus_one > 0; row_plus_one--) - { - std::size_t row = row_plus_one - 1; - double target = working.at(row, col); - double a = -target / diag; - working.combineRow(col, row, a); - result.combineRow(col, row, a); - } - } - return result; - } - - Matrix Matrix::sqrt() const - { - // The matrix square root should be calculated using numerically efficient and stable methods such as the Cholesky decomposition - if (m_rows != m_cols) - { - throw std::runtime_error("Matrix must be square."); - } - return choleskyDecomposition(); - } - - double Matrix::determinant() const - { - if (rows() != cols()) - { - throw std::runtime_error("Matrix must be square."); - } - switch (rows()) - { - case 0: - return 1; - case 1: - return m_m[0]; - case 2: - { - return m_m[0] * m_m[3] - m_m[1] * m_m[2]; - } - case 3: - { - double a = at(0, 0); - double b = at(0, 1); - double c = at(0, 2); - double d = at(1, 0); - double e = at(1, 1); - double f = at(1, 2); - double g = at(2, 0); - double h = at(2, 1); - double i = at(2, 2); - return a * e * i + b * f * g + c * d * h - a * f * h - b * d * i - - c * e * g; - } - default: - { - int swapCount; - std::vector pivot; - Matrix LU = luDecompositionByGE(&swapCount, &pivot); - double result = 1.0; - for (std::size_t k = 0; k < LU.order(); k++) - { - result *= LU.at(pivot[k], k); - } - if (swapCount % 2 == 1) - { - result = -result; - } - return result; - } - } - } - - void Matrix::scaleRow(std::size_t r, double k) - { - for (std::size_t c = 0; c < m_cols; c++) - { - m_m[getIndex(r, c)] = k * at(r, c); - } - } - - void Matrix::combineRow(std::size_t srcRow, std::size_t dstRow, double k) - { - for (std::size_t c = 0; c < m_cols; c++) - { - m_m[getIndex(dstRow, c)] = at(dstRow, c) + k * at(srcRow, c); - } - } - - Matrix Matrix::luDecompositionByGE(int* p_swap_count, - std::vector* p_pivot) const - { - if (m_rows != m_cols) - { - throw std::runtime_error("Matrix must be square."); - } - std::vector& pivot = *p_pivot; - *p_swap_count = 0; - std::size_t n = rows(); - - // Make a copy of the matrix. - // We never change the original matrix because the backing array may be reused. - std::vector a(m_m); - Matrix m(n, n, a); - - initPivots(n, p_pivot); - for (std::size_t k = 0; k < n - 1; k++) - { - double maxValue = m.partialPivot(p_pivot, k, p_swap_count); - if (maxValue == 0) - { - throw std::runtime_error("Matrix is singular."); - } - double m_kk = m.at(pivot[k], k); - for (std::size_t i = k + 1; i < n; i++) - { - std::size_t ik = m.getIndex(pivot[i], k); - m.m_m[ik] /= m_kk; - } - for (std::size_t i = k + 1; i < n; i++) - { - double m_ik = m.at(pivot[i], k); - for (std::size_t j = k + 1; j < n; j++) - { - double m_kj = m.at(pivot[k], j); - std::size_t ij = m.getIndex(pivot[i], j); - m.m_m[ij] -= m_ik * m_kj; - } - } - } - if (m.at(pivot[n - 1], n - 1) == 0) - { - throw std::runtime_error("Matrix is singular."); - } - return m; - } - - Matrix Matrix::roundSymmetry() - { - std::vector a(m_m.size()); - for (std::size_t i = 0; i < m_rows; i++) - { - std::size_t diagIndex = getIndex(i, i); - a[diagIndex] = m_m[diagIndex]; - for (std::size_t j = i + 1; j < m_cols; j++) - { - std::size_t i1 = getIndex(i, j); - std::size_t i2 = getIndex(j, i); - double mean = (m_m[i1] + m_m[i2]) / 2; - a[i1] = a[i2] = mean; - } - } - return Matrix(m_rows, m_cols, a); - } - - std::size_t Matrix::getIndex(std::size_t row, std::size_t col) const - { - if (m_row_based) - { - return row * m_cols + col; - } - else - { - return col * m_rows + row; - } - } - - /* static */ - void Matrix::initPivots(std::size_t n, std::vector* p_pivot) - { - std::vector& pivot = *p_pivot; - pivot.resize(n); - for (std::size_t i = 0; i < pivot.size(); i++) - { - pivot[i] = i; - } - } - - double Matrix::partialPivot(std::vector* p_pivot, std::size_t k, int* p_swap_count) const - { - std::vector& pivot = *p_pivot; - int& swap_count = *p_swap_count; - double maxValue = fabs(at(pivot[k], k)); - std::size_t maxIndex = k; - for (std::size_t i = k + 1; i < pivot.size(); i++) - { - std::size_t rowIndex = pivot[i]; - double rowValue = fabs(at(rowIndex, k)); - if (rowValue > maxValue) - { - maxValue = rowValue; - maxIndex = i; - } - } - if (maxIndex != k) - { - std::size_t temp = pivot[maxIndex]; - pivot[maxIndex] = pivot[k]; - pivot[k] = temp; - swap_count++; - } - return maxValue; - } - - bool Matrix::isSymmetric() const - { - if (m_rows != m_cols) - { - return false; - } - for (std::size_t i = 1; i < rows(); i++) - { - for (std::size_t j = 1; j < i; j++) - { - if (at(i, j) != at(j, i)) - { - return false; - } - } - } - return true; - } - - Matrix Matrix::choleskyDecomposition() const - { - // returns an upper diagonal matrix - if (m_rows != m_cols) - { - throw std::runtime_error("Matrix must be square."); - } - if (!isSymmetric()) - { - throw std::runtime_error("Matrix must be symmetric."); - } - std::size_t n = rows(); - std::vector a(m_m); - for (std::size_t i = 0; i < n; i++) - { - std::size_t ii = getIndex(i, i); - for (std::size_t k = 0; k < i; k++) - { - std::size_t ki = getIndex(k, i); - a[ii] = a[ii] - a[ki] * a[ki]; - } - if (a[ii] < 0) - { - throw std::runtime_error("Matrix is not positive definite."); - } - a[ii] = std::sqrt(a[ii]); - for (std::size_t j = i + 1; j < n; j++) - { - std::size_t ij = getIndex(i, j); - for (std::size_t k = 0; k < i; k++) - { - std::size_t ki = getIndex(k, i); - std::size_t kj = getIndex(k, j); - a[ij] = a[ij] - a[ki] * a[kj]; - } - if (a[ij] != 0) { a[ij] = a[ij] / a[ii]; } - } - } - // Clear out the lower matrix - for (std::size_t i = 1; i < n; i++) - { - for (std::size_t j = 0; j < i; j++) - { - std::size_t ij = getIndex(i, j); - a[ij] = 0; - } - } - return Matrix(n, n, a); - } - - std::ostream& operator<<(std::ostream& os, const Matrix& m) - { - os << m.rows() << "x" << m.cols() << ":{"; - for (std::size_t row = 0; row < m.rows(); row++) - { - if (row > 0) { os << ", "; } - os << "{"; - for (std::size_t col = 0; col < m.cols(); col++) - { - if (col > 0) { os << ", "; } - os << m.at(row, col); - } - os << "}"; - } - os << "}"; - return os; - - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/src/probability/data/matrix_vector_operators.cpp b/core/navigation/localization/src/probability/data/matrix_vector_operators.cpp deleted file mode 100644 index f1cc09f..0000000 --- a/core/navigation/localization/src/probability/data/matrix_vector_operators.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "localization/probability/data/matrix_vector_operators.hpp" -#include "localization/probability/data/matrix.hpp" -#include "localization/probability/data/vector.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - Vector operator*(const Matrix& m, const Vector& v) - { - if (m.cols() != v.order()) - { - std::stringstream error_message; - error_message << "Matrix and Vector sizes not compatible " - << "(" << m << " * " << v << ")"; - throw std::runtime_error(error_message.str()); - } - std::vector result(m.rows()); - for (std::size_t row = 0; row < m.rows(); row++) - { - double value = 0.0; - for (std::size_t col = 0; col < m.cols(); col++) - { - value += m.at(row, col) * v[col]; - } - result[row] = value; - } - return Vector(result); - } - - Vector operator*(const Vector& v, const Matrix& m) - { - if (v.order() != m.rows()) - { - throw std::runtime_error("Matrix size is not compatible with Vector size."); - } - std::vector result(v.order()); - for (std::size_t col = 0; col < m.cols(); col++) - { - for (std::size_t row = 0; row < m.rows(); row++) - { - result[row] += v[row] * m.at(row, col); - } - } - return Vector(result); - - } - - Matrix operator*(double scalar, const Matrix& m) - { - const std::vector& ma = m.m_m; - std::vector a(ma.size()); - for (std::size_t i = 0; i < a.size(); i++) - { - a[i] = ma[i] * scalar; - } - return Matrix(m.rows(), m.cols(), a); - - } - - double operator*(const Vector& v1, const Vector& v2) - { - return v1.dot(v2); - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/src/probability/data/vector.cpp b/core/navigation/localization/src/probability/data/vector.cpp deleted file mode 100644 index c351eea..0000000 --- a/core/navigation/localization/src/probability/data/vector.cpp +++ /dev/null @@ -1,209 +0,0 @@ -#include "localization/probability/data/vector.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - - Vector::Vector() - { - } - - Vector::Vector(std::vector a) : - m_a(std::move(a)) - { - } - - Vector::Vector(const Vector& v) : - m_a(v.m_a) - { - } - - Vector::Vector(Vector&& v) : - m_a(std::move(v.m_a)) - { - } - - Vector::Vector(std::size_t order) : - m_a(order) - { - } - - Vector::~Vector() - { - } - - double Vector::dot(const Vector& v2) const - { - const Vector& v1 = *this; - double result = 0.0; - for (std::size_t i = 0; i < v1.order(); i++) - { - result += v1.m_a[i] * v2.m_a[i]; - } - return result; - - } - - std::size_t Vector::order() const - { - return m_a.size(); - } - - void Vector::set(Vector a) - { - if (order() != a.order()) - { - throw std::runtime_error( - "Length of array is different from the Vector expected order."); - } - m_a = std::move(a.m_a); - } - - double& Vector::at(int i) - { - return m_a[i]; - } - - double Vector::at(int i) const - { - return m_a[i]; - } - - Vector Vector::operator+(const Vector& v2) const - { - const Vector& v1 = *this; - if (v1.order() != v2.order()) - { - throw std::runtime_error("Vector sizes are not compatible."); - } - Vector result(std::vector(v1.order())); - std::vector& a = result.m_a; - for (std::size_t i = 0; i < a.size(); i++) - { - a[i] = v1.m_a[i] + v2.m_a[i]; - } - return result; - } - - Vector Vector::operator-(const Vector& v2) const - { - const Vector& v1 = *this; - if (v1.order() != v2.order()) - { - throw std::runtime_error("Vector sizes are not compatible."); - } - Vector result(v1.order()); - std::vector& a = result.m_a; - for (std::size_t i = 0; i < a.size(); i++) - { - a[i] = v1.m_a[i] - v2.m_a[i]; - } - return result; - } - - bool Vector::operator<(const Vector& v2) const - { - if (this == &v2) { return false; } - const Vector& v1 = *this; - bool result = v1.order() > 0; - for (std::size_t i = 0; i < v1.order(); i++) - { - result = result && v1.m_a[i] < v2.m_a[i]; - } - return result; - } - - bool Vector::operator>(const Vector& v2) const - { - if (this == &v2) { return false; } - const Vector& v1 = *this; - bool result = v1.order() > 0; - for (std::size_t i = 0; i < v1.order(); i++) - { - result = result && v1.m_a[i] > v2.m_a[i]; - } - return result; - } - - bool Vector::operator==(const Vector& v) const - { - if (this == &v) { return true; } - return m_a == v.m_a; - } - - Vector& Vector::operator=(const Vector& that) - { - this->m_a = that.m_a; - return *this; - } - -// operator= is implicitly declared as deleted in subclasses -// because it also defines a move assignment operator. - void Vector::copyAssign(const Vector& v) - { - if (this != &v) - { - m_a = v.m_a; - } - } - - void Vector::moveAssign(Vector&& v) - { - if (this != &v) - { - m_a = std::move(v.m_a); - } - } - - double& Vector::operator[](std::size_t i) - { - return m_a[i]; - } - - double Vector::operator[](std::size_t i) const - { - return m_a[i]; - } - - Matrix Vector::cross(const Vector& that) const - { - std::vector m(order() * that.order()); - int k = 0; - for (std::size_t i = 0; i < order(); i++) - { - double a = m_a[i]; - for (std::size_t j = 0; j < that.order(); j++) - { - double b = that[j]; - m[k] = a * b; - k++; - } - } - return Matrix(order(), that.order(), m); - } - - Vector Vector::aliasVector() const - { - return Vector(m_a); - } - - std::ostream& operator<<(std::ostream& os, const Vector& v) - { - os << v.order() << ":{"; - for (std::size_t i = 0; i < v.order(); i++) - { - if (i > 0) { os << ", "; } - os << v[i]; - } - os << "}"; - return os; - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/test/main.cpp b/core/navigation/localization/test/main.cpp deleted file mode 100644 index ce0f6f1..0000000 --- a/core/navigation/localization/test/main.cpp +++ /dev/null @@ -1,8 +0,0 @@ - -#include "gtest/gtest.h" - -int main(int argc, char** argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} \ No newline at end of file diff --git a/core/navigation/localization/test/test_bayes_distribution.cpp b/core/navigation/localization/test/test_bayes_distribution.cpp deleted file mode 100644 index a37cc21..0000000 --- a/core/navigation/localization/test/test_bayes_distribution.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include - -#include "localization/probability/random_conditional.hpp" -#include "localization/probability/ranged_uniform.hpp" -#include "localization/probability/bayes_distribution.hpp" -#include "localization/sensor_model.hpp" - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - namespace test - { - DEFINE_VECTOR1(X); - DEFINE_VECTOR1(Z); - - /** - * A sensor model that says that X is within 0.25 units observed Z - */ - class SensorModel : public RandomConditional - { - public: - double conditionalProbabilityOf(const Z& z, const X& x) const override - { - if(fabs(z[0] - x[0]) < 0.25) - { - return 2.0; - } - return 0.0; - } - }; - - typedef BayesDistribution BayesXZ; - - TEST(BayesDistributionTest, probabilityOf) - { - auto belief = std::make_shared>(X({0}), X({2})); - auto sensor_model = std::make_shared(); - auto bd = std::make_shared(belief, sensor_model, 0.5); - // Probabilities are not normalized. They should be treated proportionally - // and not as exact probabilities. - EXPECT_EQ(0.0, bd->probabilityOf(0.1)); - EXPECT_EQ(1.0, bd->probabilityOf(0.3)); - EXPECT_EQ(1.0, bd->probabilityOf(0.5)); - EXPECT_EQ(1.0, bd->probabilityOf(0.7)); - EXPECT_EQ(0.0, bd->probabilityOf(0.9)); - } - - TEST(BayesDistributionTest, sample) - { - std::default_random_engine generator(0); - auto belief = std::make_shared< RangedUniform >(X({0}), X({2})); - auto sensor_model = std::make_shared(); - auto bd = std::make_shared(belief, sensor_model, 0.5); - // Sample is not implemented in Bayes Distribution - ASSERT_THROW(bd->sample(&generator), std::runtime_error); - } - - TEST(BayesDistributionTest, chainObservation) - { - auto belief = std::make_shared>(X({0}), X({2})); - auto sensor_model = std::make_shared(); - auto bd = std::make_shared(belief, sensor_model, 0.5); - bd = bd->chainObservation(0.9); - // Probabilities are not normalized. They should be treated proportionally - // and not as exact probabilities. - EXPECT_EQ(0.0, bd->probabilityOf(0.1)); - EXPECT_EQ(0.0, bd->probabilityOf(0.3)); - EXPECT_EQ(0.0, bd->probabilityOf(0.5)); - EXPECT_EQ(2.0, bd->probabilityOf(0.7)); - EXPECT_EQ(0.0, bd->probabilityOf(0.9)); - } - - TEST(BayesDistributionTest, estimateNormalizer) - { - auto belief = std::make_shared< RangedUniform >(X({0}), X({2})); - auto sensor_model = std::make_shared(); - auto bd = std::make_shared(belief, sensor_model, 0.5); - std::vector samples; - double dx = 0.02; - samples.reserve((long)(4 / dx)); - for(double x = -1.0; x < 3.0; x += dx) - { - samples.push_back(x); - } - EXPECT_EQ(0.5, bd->estimateNormalizer(samples, 0.02)); - EXPECT_EQ(0.5, bd->normalizer()); - EXPECT_EQ(0.0, bd->probabilityOf(0.1)); - EXPECT_EQ(2.0, bd->probabilityOf(0.3)); - EXPECT_EQ(2.0, bd->probabilityOf(0.5)); - EXPECT_EQ(2.0, bd->probabilityOf(0.7)); - EXPECT_EQ(0.0, bd->probabilityOf(0.9)); - } - - TEST(BayesDistributionTest, properties) - { - auto belief = std::make_shared>(X({0}), X({2})); - auto sensor_model = std::make_shared(); - auto bd = std::make_shared(belief, sensor_model, 0.5); - EXPECT_EQ(sensor_model.get(), &bd->sensorModel()); - EXPECT_EQ(Z(0.5), bd->data()); - } - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/test/test_conditional_map.hpp.cpp b/core/navigation/localization/test/test_conditional_map.hpp.cpp deleted file mode 100644 index 5fd6cd4..0000000 --- a/core/navigation/localization/test/test_conditional_map.hpp.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include -#include -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - namespace test - { - DEFINE_VECTOR1(X); - enum Y - { - y0, - y1 - }; - - TEST(ConditionalMapTest, GetSet) { - ConditionalMap px_given_y; - px_given_y.set(y0, std::make_shared>(X(0.5), X(1.0))); - px_given_y.set(y1, std::make_shared>(X(0.0), X(2.0))); - EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(0.4, y0)); - EXPECT_EQ(2.0, px_given_y.conditionalProbabilityOf(0.8, y0)); - EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(1.5, y0)); - EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(-1.0, y1)); - EXPECT_EQ(0.5, px_given_y.conditionalProbabilityOf(1.5, y1)); - EXPECT_EQ(0.0, px_given_y.conditionalProbabilityOf(3.0, y1)); - } - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/test/test_conditional_value_map.cpp b/core/navigation/localization/test/test_conditional_value_map.cpp deleted file mode 100644 index 5c1ec45..0000000 --- a/core/navigation/localization/test/test_conditional_value_map.cpp +++ /dev/null @@ -1,132 +0,0 @@ -#include -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - namespace test - { - enum class X - { - x0, x1 - }; - - enum class Y - { - y0, y1 - }; - - enum class Z - { - z0, z1 - }; - - typedef ConditionalValueMap CvmXY; - typedef ConditionalValueMap CvmYZ; - - TEST(ConditionalValueMapTest, DomainRange) - { - CvmXY xy; - xy.set(X::x0, Y::y0, 1.0); - xy.set(X::x1, Y::y0, 1.0); - EXPECT_EQ(std::vector({X::x0, X::x1}), xy.domain()); - EXPECT_EQ(std::vector({Y::y0}), xy.range()); - } - - TEST(ConditionalValueMapTest, SetGet) - { - CvmXY xy; - xy.set(X::x0, Y::y0, 0.9); - xy.set(X::x1, Y::y0, 0.1); - xy.set(X::x0, Y::y1, 0.2); - xy.set(X::x1, Y::y1, 0.8); - EXPECT_EQ(0.9, xy.conditionalProbabilityOf(X::x0, Y::y0)); - EXPECT_EQ(0.1, xy.conditionalProbabilityOf(X::x1, Y::y0)); - EXPECT_EQ(0.2, xy.conditionalProbabilityOf(X::x0, Y::y1)); - EXPECT_EQ(0.8, xy.conditionalProbabilityOf(X::x1, Y::y1)); - } - - TEST(ConditionalValueMapTest, LikelihoodOf) - { - CvmXY xy; - xy.set(X::x0, Y::y0, 0.9); - xy.set(X::x1, Y::y0, 0.1); - xy.set(X::x0, Y::y1, 0.2); - xy.set(X::x1, Y::y1, 0.8); - - auto lh0 = xy.likelihoodOf(X::x0); - EXPECT_DOUBLE_EQ(0.9, lh0->probabilityOf(Y::y0)); - EXPECT_DOUBLE_EQ(0.2, lh0->probabilityOf(Y::y1)); - lh0->normalize(); - EXPECT_DOUBLE_EQ(9.0 / 11.0, lh0->probabilityOf(Y::y0)); - EXPECT_DOUBLE_EQ(2.0 / 11.0, lh0->probabilityOf(Y::y1)); - - auto lh1 = xy.likelihoodOf(X::x1); - EXPECT_DOUBLE_EQ(0.1, lh1->probabilityOf(Y::y0)); - EXPECT_DOUBLE_EQ(0.8, lh1->probabilityOf(Y::y1)); - lh1->normalize(); - EXPECT_DOUBLE_EQ(1.0 / 9.0, lh1->probabilityOf(Y::y0)); - EXPECT_DOUBLE_EQ(8.0 / 9.0, lh1->probabilityOf(Y::y1)); - } - - TEST(ConditionalValueMapTest, BayesianInference) - { - CvmXY xy; - xy.set(X::x0, Y::y0, 0.9); - xy.set(X::x1, Y::y0, 0.1); - xy.set(X::x0, Y::y1, 0.2); - xy.set(X::x1, Y::y1, 0.8); - DistributionValueMap belief0(Y::y0, 0.1, Y::y1, 0.9); - auto belief1 = xy.bayesianInference(X::x0, belief0); - EXPECT_DOUBLE_EQ(9.0 / 27.0, belief1->probabilityOf(Y::y0)); - EXPECT_DOUBLE_EQ(18.0 / 27.0, belief1->probabilityOf(Y::y1)); - } - - TEST(ConditionalValueMapTest, Marginalize) - { - CvmXY xy; - xy.set(X::x0, Y::y0, 0.9); - xy.set(X::x1, Y::y0, 0.1); - xy.set(X::x0, Y::y1, 0.2); - xy.set(X::x1, Y::y1, 0.8); - - CvmYZ yz; - yz.set(Y::y0, Z::z0, 0.9); - yz.set(Y::y1, Z::z0, 0.1); - yz.set(Y::y0, Z::z1, 0.2); - yz.set(Y::y1, Z::z1, 0.8); - - auto xz = xy.marginalize(yz); - EXPECT_DOUBLE_EQ(0.83, xz->conditionalProbabilityOf(X::x0, Z::z0)); - EXPECT_DOUBLE_EQ(0.17, xz->conditionalProbabilityOf(X::x1, Z::z0)); - EXPECT_DOUBLE_EQ(0.34, xz->conditionalProbabilityOf(X::x0, Z::z1)); - EXPECT_DOUBLE_EQ(0.66, xz->conditionalProbabilityOf(X::x1, Z::z1)); - } - - TEST(ConditionalValueMapTest, SampleLikelihood) - { - CvmXY xy; - xy.set(X::x0, Y::y0, 0.9); - xy.set(X::x1, Y::y0, 0.1); - xy.set(X::x0, Y::y1, 0.2); - xy.set(X::x1, Y::y1, 0.8); - - EXPECT_EQ(1.1, xy.sumLikelihood(X::x0)); - EXPECT_EQ(0.9, xy.sumLikelihood(X::x1)); - - EXPECT_EQ(Y::y0, xy.sampleLikelihood(X::x0, 0.85)); - EXPECT_EQ(Y::y1, xy.sampleLikelihood(X::x0, 0.95)); - EXPECT_EQ(Y::y0, xy.sampleLikelihood(X::x1, 0.05)); - EXPECT_EQ(Y::y1, xy.sampleLikelihood(X::x1, 0.15)); - } - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/test/test_independent_pair_distribution.cpp b/core/navigation/localization/test/test_independent_pair_distribution.cpp deleted file mode 100644 index 6d074f5..0000000 --- a/core/navigation/localization/test/test_independent_pair_distribution.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include - -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - namespace test - { - enum X { x0, x1 }; - enum Y { y0, y1 }; - typedef std::pair XY; - - TEST(IndependentPairDistributionTest, ProbabilityOf) - { - auto x = std::make_shared>(x0, 0.1, x1, 0.9); - auto y = std::make_shared>(y0, 0.2, y1, 0.8); - auto xy = std::make_shared>(x, y); - EXPECT_DOUBLE_EQ(0.02, xy->probabilityOf(XY(x0, y0))); - EXPECT_DOUBLE_EQ(0.08, xy->probabilityOf(XY(x0, y1))); - EXPECT_DOUBLE_EQ(0.18, xy->probabilityOf(XY(x1, y0))); - EXPECT_DOUBLE_EQ(0.72, xy->probabilityOf(XY(x1, y1))); - } - - TEST(IndependentPairDistributionTest, Sample) - { - std::default_random_engine generator(0); - auto x = std::make_shared>(x0, 0.1, x1, 0.9); - auto y = std::make_shared>(y0, 0.2, y1, 0.8); - auto xy = std::make_shared>(x, y); - double x0y0_count = 0, x1y0_count = 0, x0y1_count = 0, x1y1_count = 0; - int n = 1000; - for (int i = 0; i < n; ++i) - { - auto sample = xy->sample(&generator); - if (sample == XY(x0, y0)) - { - x0y0_count++; - } - - if (sample == XY(x0, y1)) - { - x0y1_count++; - } - if (sample == XY(x1, y0)) - { - x1y0_count++; - } - if (sample == XY(x1, y1)) - { - x1y1_count++; - } - } - std::cout << "Selection counts: " - << x0y0_count << " " - << x0y1_count << " " - << x1y0_count << " " - << x1y1_count << std::endl; - EXPECT_TRUE(x0y0_count > 10 && x0y0_count < 30); - EXPECT_TRUE(x0y1_count > 40 && x0y0_count < 120); - EXPECT_TRUE(x1y0_count > 90 && x0y0_count < 270); - EXPECT_TRUE(x1y1_count > 620 && x0y0_count < 820); - } - - } - } - } - } -} \ No newline at end of file diff --git a/core/navigation/localization/test/test_random_distribution.cpp b/core/navigation/localization/test/test_random_distribution.cpp deleted file mode 100644 index e5f8f52..0000000 --- a/core/navigation/localization/test/test_random_distribution.cpp +++ /dev/null @@ -1,46 +0,0 @@ -#include -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - namespace test - { - - DEFINE_VECTOR1(X); - - class Impulse : RandomDistribution - { - public: - double probabilityOf(const X& x) const override - { - if(x[0] == 0.0) - return 1.0; - return 0.0; - } - - X sample(std::default_random_engine*) const override - { - return 0.0; - } - }; - - TEST(RandomDistributionTest, ctor1) - { - std::default_random_engine generator(0); - Impulse d; - ASSERT_EQ(1.0, d.probabilityOf(0.0)); - ASSERT_EQ(0.0, d.probabilityOf(0.5)); - ASSERT_EQ(X(0.0), d.sample(&generator)); - } - } - } - } - } -} diff --git a/core/navigation/localization/test/test_ranged_uniform.cpp b/core/navigation/localization/test/test_ranged_uniform.cpp deleted file mode 100644 index 6629154..0000000 --- a/core/navigation/localization/test/test_ranged_uniform.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include -#include -#include -#include - -namespace rip -{ - namespace navigation - { - namespace localization - { - namespace probability - { - namespace test - { - DEFINE_VECTOR1(X); - - TEST(RangedUniformTest, ProbabilityOf) - { - auto x = std::make_shared> - (X({0.25}), X({0.75})); - EXPECT_EQ(0.0, x->probabilityOf(0.1)); - EXPECT_EQ(2.0, x->probabilityOf(0.3)); - EXPECT_EQ(2.0, x->probabilityOf(0.5)); - EXPECT_EQ(2.0, x->probabilityOf(0.7)); - EXPECT_EQ(0.0, x->probabilityOf(0.9)); - } - - TEST(RangedUniformTest, Sample) - { - std::default_random_engine generator(0); - auto x = std::make_shared> - (X({0.25}), X({0.75})); - for(int i = 0; i < 1000; i ++) - { - X sample = x->sample(&generator); - EXPECT_TRUE(sample[0] < 0.75 && sample[0] > 0.25); - } - } - } - } - } - } -} diff --git a/core/navigation/pose/CMakeLists.txt b/core/navigation/pose/CMakeLists.txt new file mode 100644 index 0000000..b083799 --- /dev/null +++ b/core/navigation/pose/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.1) +project(pose) + +# Get all .cpp files except for main.cpp +file(GLOB_RECURSE ${PROJECT_NAME}_HEADERS "include/pose/*.hpp") +file(GLOB_RECURSE ${PROJECT_NAME}_SOURCES "src/*.cpp") + +# Create the library, set to c++11, link external libraries, and set include dir +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_SOURCES} ${${PROJECT_NAME}_HEADERS}) +set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) +if(ENABLE_TESTING) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -O0 -fprofile-arcs -ftest-coverage") +endif() +target_link_libraries(${PROJECT_NAME} units spdlog) +target_include_directories(${PROJECT_NAME} PUBLIC include) + +if(ENABLE_TESTING) + # Get test source files and test data files + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_SOURCES "test/*.cpp") + file(GLOB_RECURSE ${PROJECT_NAME}_TEST_DATA "test/data/*") + + # Copy test data to outdir/test + include(FileOutputs) + make_outputs(${CMAKE_CURRENT_SOURCE_DIR} "${${PROJECT_NAME}_TEST_DATA}" ${CMAKE_CURRENT_BINARY_DIR} testDataOutputs) + + # Create arduino_gen_test executable, set to c++11, link navx library and google test libraries + add_executable(${PROJECT_NAME}_test ${${PROJECT_NAME}_TEST_SOURCES} ${testDataOutputs}) + set_property(TARGET ${PROJECT_NAME}_test PROPERTY CXX_STANDARD 11) + target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) + target_link_libraries(${PROJECT_NAME}_test gmock gtest googletest_rip_macros) + +endif() diff --git a/core/navigation/pose/cmake_modules/FindEigen3.cmake b/core/navigation/pose/cmake_modules/FindEigen3.cmake new file mode 100644 index 0000000..800594f --- /dev/null +++ b/core/navigation/pose/cmake_modules/FindEigen3.cmake @@ -0,0 +1,87 @@ +# - Try to find Eigen3 lib +# +# This module supports requiring a minimum version, e.g. you can do +# find_package(Eigen3 3.1.2) +# to require version 3.1.2 or newer of Eigen3. +# +# Once done this will define +# +# EIGEN3_FOUND - system has eigen lib with correct version +# EIGEN3_INCLUDE_DIR - the eigen include directory +# EIGEN3_VERSION - eigen version + +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD license. + +if(NOT Eigen3_FIND_VERSION) + if(NOT Eigen3_FIND_VERSION_MAJOR) + set(Eigen3_FIND_VERSION_MAJOR 2) + endif(NOT Eigen3_FIND_VERSION_MAJOR) + if(NOT Eigen3_FIND_VERSION_MINOR) + set(Eigen3_FIND_VERSION_MINOR 91) + endif(NOT Eigen3_FIND_VERSION_MINOR) + if(NOT Eigen3_FIND_VERSION_PATCH) + set(Eigen3_FIND_VERSION_PATCH 0) + endif(NOT Eigen3_FIND_VERSION_PATCH) + + set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") +endif(NOT Eigen3_FIND_VERSION) + +macro(_eigen3_check_version) + file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) + + string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") + set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") + set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") + string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") + set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") + + set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) + if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK FALSE) + else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + set(EIGEN3_VERSION_OK TRUE) + endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) + + if(NOT EIGEN3_VERSION_OK) + + message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " + "but at least version ${Eigen3_FIND_VERSION} is required") + endif(NOT EIGEN3_VERSION_OK) +endmacro(_eigen3_check_version) + +if (EIGEN3_INCLUDE_DIR) + + # in cache already + _eigen3_check_version() + set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) + +else (EIGEN3_INCLUDE_DIR) + + # specific additional paths for some OS + if (WIN32) + set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") + endif(WIN32) + + find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library + PATHS + ${CMAKE_INSTALL_PREFIX}/include + ${EIGEN_ADDITIONAL_SEARCH_PATHS} + ${KDE4_INCLUDE_DIR} + PATH_SUFFIXES eigen3 eigen + ) + + if(EIGEN3_INCLUDE_DIR) + _eigen3_check_version() + endif(EIGEN3_INCLUDE_DIR) + + include(FindPackageHandleStandardArgs) + find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) + + mark_as_advanced(EIGEN3_INCLUDE_DIR) + +endif(EIGEN3_INCLUDE_DIR) + diff --git a/core/navigation/pose/include/pose/action_model.hpp b/core/navigation/pose/include/pose/action_model.hpp new file mode 100644 index 0000000..1e8b5a9 --- /dev/null +++ b/core/navigation/pose/include/pose/action_model.hpp @@ -0,0 +1,26 @@ +#ifndef ACTION_MODEL_HPP +#define ACTION_MODEL_HPP + +namespace rip +{ + namespace navigation + { + namespace pose + { + template + class ActionModel + { + public: + ActionModel() + { + } + + virtual ~ActionModel() + { + } + }; + } + } +} + +#endif //ACTION_MODEL_HPP diff --git a/core/navigation/pose/include/pose/extended_kalman_filter.hpp b/core/navigation/pose/include/pose/extended_kalman_filter.hpp new file mode 100644 index 0000000..99581cc --- /dev/null +++ b/core/navigation/pose/include/pose/extended_kalman_filter.hpp @@ -0,0 +1,136 @@ +#ifndef EXTENDED_KALMAN_FILTER_HPP +#define EXTENDED_KALMAN_FILTER_HPP + +#include "matrix.hpp" +#include "vector.hpp" +#include "gaussian_moment.hpp" +#include "gaussian_action_model.hpp" +#include "gaussian_sensor_model.hpp" + +#include + +namespace rip +{ + namespace navigation + { + namespace pose + { + template + class ExtendedKalmanActionModel : public GaussianActionModel + { + public: + ExtendedKalmanActionModel() + { + } + + virtual ~ExtendedKalmanActionModel() + { + } + + virtual Matrix getStateJacobian(const U& action, const X& state) const = 0; + + virtual Matrix getActionJacobian(const U& action, const X& state) const = 0; + }; + + template + class ExtendedKalmanSensorModel : public GaussianSensorModel + { + public: + ExtendedKalmanSensorModel() + { + } + + virtual ~ExtendedKalmanSensorModel() + { + } + + virtual Matrix getJacobian(const X& x) const = 0; + }; + + template + class ExtendedKalmanFilter + { + public: + ExtendedKalmanFilter() + { + } + + virtual ~ExtendedKalmanFilter() + { + } + + std::shared_ptr > + marginalize(const ExtendedKalmanActionModel& model, const U& action, + const GaussianMoment & belief) + { + + X x0 = belief.mean(); + Matrix e0 = belief.covariance(); + + // time update + Matrix g = model.GetStateJacobian(action, x0); + Matrix gt = g.Transpose(); + Matrix r = model.GetError(action, x0); + + X x1 = model.GetMean(action, x0); + Matrix e1 = g * e0 * gt + r; + + return std::make_shared < GaussianMoment < X >> (x1, e1); + } + + std::shared_ptr > + marginalize(const ExtendedKalmanActionModel& model, const GaussianMoment & action, + const GaussianMoment & belief) + { + + X x0 = belief.mean(); + U u = action.mean(); + Matrix e0 = belief.covariance(); + Matrix eu = action.covariance(); + + // time update + Matrix gx = model.GetStateJacobian(u, x0); + Matrix gxt = gx.Transpose(); + Matrix gu = model.GetActionJacobian(action.Mean, x0); + Matrix gut = gu.Transpose(); + Matrix r = model.GetError(u, x0); + + X x1 = model.GetMean(u, x0); + Matrix e1 = gx * e0 * gxt + gu * eu * gut + r; + + return std::make_shared < GaussianMoment < X >> (x1, e1); + } + + + std::shared_ptr > + bayesianInference(const ExtendedKalmanSensorModel& sensor_model, const Z& observation, + const GaussianMoment & belief) + { + + // Table 3.3 p59 + X x0 = belief.mean(); + Matrix e0 = belief.covariance(); + + Z expected_z = sensor_model.GetMean(x0); + Matrix q = sensor_model.GetError(observation); + + Matrix h = sensor_model.GetJacobian(x0); + Matrix ht = h.Transpose(); + + Matrix i = Matrix::Identity(e0.order()); + + Matrix e0_ht = e0 * ht; + Matrix k = e0_ht * (h * e0_ht + q).Inverse(); + Vector v = (x0 + k * (observation - expected_z)); + X x1; + x1.Set(std::move(v)); + Matrix e1 = (i - k * h) * e0; + + return std::make_shared < GaussianMoment < X >> (x1, e1); + } + }; + } + } +} + +#endif //EXTENDED_KALMAN_FILTER_HPP \ No newline at end of file diff --git a/core/navigation/localization/include/localization/gaussian_action_model.hpp b/core/navigation/pose/include/pose/gaussian_action_model.hpp similarity index 67% rename from core/navigation/localization/include/localization/gaussian_action_model.hpp rename to core/navigation/pose/include/pose/gaussian_action_model.hpp index 7d4a23b..dac4297 100644 --- a/core/navigation/localization/include/localization/gaussian_action_model.hpp +++ b/core/navigation/pose/include/pose/gaussian_action_model.hpp @@ -1,26 +1,30 @@ #ifndef GAUSSIAN_ACTION_MODEL_HPP #define GAUSSIAN_ACTION_MODEL_HPP -#include "probability/data/matrix.hpp" -#include "action_model.hpp" +#include "pose/matrix.hpp" +#include "pose/action_model.hpp" namespace rip { namespace navigation { - namespace localization + namespace pose { template class GaussianActionModel : public ActionModel { - using probability::Matrix; public: - GaussianActionModel() { + GaussianActionModel() + { + } + + virtual ~GaussianActionModel() + { } - virtual ~GaussianActionModel() {} // Expected next state. virtual X getMean(const U& action, const X& state) const = 0; + // Process noise. virtual Matrix getError(const U& action, const X& mean) const = 0; }; @@ -28,4 +32,4 @@ namespace rip } } -#endif // GAUSSIAN_ACTION_MODEL_HPP \ No newline at end of file +#endif //GAUSSIAN_ACTION_MODEL_HPP diff --git a/core/navigation/pose/include/pose/gaussian_moment.hpp b/core/navigation/pose/include/pose/gaussian_moment.hpp new file mode 100644 index 0000000..eb1505d --- /dev/null +++ b/core/navigation/pose/include/pose/gaussian_moment.hpp @@ -0,0 +1,146 @@ +#ifndef GAUSSIAN_MOMENT_HPP +#define GAUSSIAN_MOMENT_HPP + +#include +#include + +#include "pose/matrix.hpp" +#include "pose/vector.hpp" +#include "pose/matrix_vector_operators.hpp" +#include "pose/random_distribution.hpp" + +namespace rip +{ + namespace navigation + { + namespace pose + { + template + class GaussianMoment : public RandomDistribution + { + public: + static double Fn(double x, double mean, double variance) + { + double A = std::sqrt(1.0 / (2 * M_PI * variance)); + double dx = x - mean; + double x2 = dx * dx; + double e = std::exp(- 0.5 * x2 / variance); + return A * e; + } + + static double + Fn(Vector x, Vector mean, Matrix covariance, Matrix inverse_covariance) + { + double A = sqrt(1.0 / ((2 * M_PI) * covariance).determinant()); + Vector dx = x - mean; + double e = exp(- (dx * inverse_covariance * dx)); + return A * e; + } + + static double Fn(Vector x, Vector mean, Matrix covariance) + { + return Fn(x, mean, covariance, covariance.inverse()); + } + + GaussianMoment(X mean, Matrix covariance) + : m_mean(std::move(mean)) + , m_covariance(std::move(covariance)) + { + } + + GaussianMoment(GaussianMoment&& that) + : m_mean(std::move(that.m_mean)) + , m_covariance(std::move(that.m_covariance)) + { + } + + GaussianMoment& operator=(GaussianMoment&& that) + { + this->m_mean = std::move(that.m_mean); + this->m_covariance = std::move(that.m_covariance); + this->m_inverse_covariance = std::move(that.m_inverse_covariance); + this->m_srqt_covariance = std::move(that.m_srqt_covariance); + this->m_is_valid_inverse = m_is_valid_inverse; + this->m_is_valid_sqrt = m_is_valid_sqrt; + return *this; + } + + virtual ~GaussianMoment() + { + } + + double probabilityOf(const X& x) const override + { + return Fn(x.aliasVector(), mean().aliasVector(), covariance(), inverseCovariance()); + } + + X sample(std::default_random_engine* generator) const override + { + std::normal_distribution n01(0, 1); + Vector sample(order()); + for(std::size_t i = 0; i < sample.order(); i ++) + { + // need a N(0,1) here + sample[i] = n01(*generator); + } + X result; + result.set(mean() + sqrtCovariance() * sample); + return result; + } + + std::size_t order() const + { + return mean().order(); + } + + const X& mean() const + { + return m_mean; + } + + const Matrix& covariance() const + { + return m_covariance; + } + + const Matrix& inverseCovariance() const + { + if(! m_is_valid_inverse) + { + m_inverse_covariance = m_covariance.inverse(); + m_is_valid_inverse = true; + } + return m_inverse_covariance; + } + + const Matrix& sqrtCovariance() const + { + if(! m_is_valid_sqrt) + { + m_srqt_covariance = covariance().sqrt(); + m_is_valid_sqrt = true; + } + return m_srqt_covariance; + } + + private: + X m_mean; + Matrix m_covariance; + // These values are mutable because they are cached derivations of covariance_ + mutable Matrix m_inverse_covariance; + mutable Matrix m_srqt_covariance; + mutable bool m_is_valid_inverse = false; + mutable bool m_is_valid_sqrt = false; + }; + + template + static std::ostream& operator<<(std::ostream& os, const GaussianMoment& x) + { + os << "mu=" << x.mean() << ", sigma=" << x.covariance(); + return os; + } + } + } +} + +#endif //GAUSSIAN_MOMENT_HPP diff --git a/core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp b/core/navigation/pose/include/pose/gaussian_sensor_model.hpp similarity index 60% rename from core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp rename to core/navigation/pose/include/pose/gaussian_sensor_model.hpp index f4d0734..23c1ba8 100644 --- a/core/navigation/localization/include/localization/gaussian_sensor_model.hpp.hpp +++ b/core/navigation/pose/include/pose/gaussian_sensor_model.hpp @@ -1,27 +1,33 @@ #ifndef GAUSSIAN_SENSOR_MODEL_HPP #define GAUSSIAN_SENSOR_MODEL_HPP -#include "probability/data/matrix.hpp" -#include "sensor_model.hpp" +#include "pose/matrix.hpp" +#include "pose/sensor_model.hpp" namespace rip { namespace navigation { - namespace localization + namespace pose { template class GaussianSensorModel : public SensorModel { - using probability::Matrix; public: - GaussianSensorModel() {} - virtual ~GaussianSensorModel() override {} + GaussianSensorModel() + { + } + + virtual ~GaussianSensorModel() override + { + } virtual Z getMean(const X& state) const = 0; + virtual Matrix getError(const Z& observation) const = 0; }; } } } -#endif // GAUSSIAN_SENSOR_MODEL_HPP \ No newline at end of file + +#endif //GAUSSIAN_SENSOR_MODEL_HPP diff --git a/core/navigation/localization/include/localization/kalman_filter.hpp b/core/navigation/pose/include/pose/kalman_filter.hpp similarity index 63% rename from core/navigation/localization/include/localization/kalman_filter.hpp rename to core/navigation/pose/include/pose/kalman_filter.hpp index 7c83485..9d1049b 100644 --- a/core/navigation/localization/include/localization/kalman_filter.hpp +++ b/core/navigation/pose/include/pose/kalman_filter.hpp @@ -1,13 +1,6 @@ #ifndef KALMAN_FILTER_HPP #define KALMAN_FILTER_HPP -#include "probability/data/vector.hpp" -#include "probability/data/matrix.hpp" -#include "gaussian_action_model.hpp" -#include "gaussian_sensor_model.hpp.hpp" -#include "probability/continuous/gaussian_moment.hpp" - - #include #include @@ -15,9 +8,8 @@ namespace rip { namespace navigation { - namespace localization + namespace pose { - using probability::Vector; template class KalmanActionModel : public GaussianActionModel @@ -31,14 +23,14 @@ namespace rip { } - virtual ~KalmanActionModel() override + ~KalmanActionModel() override { } X getMean(const U& action, const X& state) const override { X result; - result.set((a() * state) + (b() * action) + c()); + result.Set((a() * state) + (b() * action) + c()); return result; } @@ -47,7 +39,7 @@ namespace rip return m_r; } - const Matrix& a() const + const =Matrix& a() const { // n x n return m_a; @@ -98,41 +90,42 @@ namespace rip }; template - class KalmanSensorModel : public GaussianSensorModel{ + class KalmanSensorModel : public GaussianSensorModel + { public: - KalmanSensorModel(int x_order, int z_order) - : m_c(x_order, z_order) - , m_d(x_order) - , m_q(z_order, z_order) { + KalmanSensorModel(int x_order, int z_order) : m_c(x_order, z_order), m_d(x_order), m_q(z_order, z_order) + { + } + + ~KalmanSensorModel() override + { } - virtual ~KalmanSensorModel() override {} - double conditionalProbabilityOf(const Z& observation, const X& state) const override + double ConditionalProbabilityOf(const Z& observation, const X& state) const override { - using probability::GaussianMoment; // TODO: Gaussian moment caches a bunch of stuff. I should store one here // instead of generating and solving one each time. return GaussianMoment(getMean(state), getError(observation)).probabilityOf(observation); } - Z getMean(const X& state) const override + Z GetMean(const X& state) const override { Z z; z.set(m_c * state + m_d); return z; } - Matrix getError(const Z& ) const override + Matrix GetError(const Z&) const override { return m_q; } - const Matrix& c() const + const math::data::Matrix& c() const { return m_c; } - const Vector& d() const + const math::data::Vector& d() const { return m_d; } @@ -166,69 +159,75 @@ namespace rip template class KalmanFilter { - using probability::GaussianMoment; - using probability::Matrix; public: - KalmanFilter() {} + KalmanFilter() + { + } - virtual ~KalmanFilter() {} + virtual ~KalmanFilter() + { + } - std::shared_ptr> bayesianInference( - const KalmanSensorModel& model, - const Z& data, - const GaussianMoment& belief) const + std::shared_ptr> bayesianInference( + const KalmanSensorModel& model, + const Z& data, + const GaussianMoment & belief + ) const { // Table 3.1 p42 auto& z = data; auto& mu0 = belief.mean(); auto& sigma0 = belief.covariance(); - auto z_est = model.getMean(mu0); - auto Q = model.getError(z); + auto z_est = model.GetMean(mu0); + auto Q = model.GetError(z); Matrix C = model.c(); Matrix K; { - Matrix sc = sigma0 * C.transpose(); - K = sc * ((C * sc) + Q).inverse(); + Matrix sc = sigma0 * C.Transpose(); + K = sc * ((C * sc) + Q).Inverse(); } X mu1; mu1.Set(mu0 + K * (z - z_est)); - Matrix I = Matrix::identity(sigma0.rows()); + Matrix I = Matrix::Identity(sigma0.rows()); Matrix sigma1 = (I - K * C) * sigma0; - return std::make_shared>(mu1, sigma1); + return std::make_shared> + (mu1, sigma1); } - std::shared_ptr> marginalize(const KalmanActionModel& model, - const U& action, - const GaussianMoment& state) const - { + std::shared_ptr> marginalize( + const KalmanActionModel& model, + const U& action, + const GaussianMoment & state + ) const { // Table 3.1 p42 const X& x1 = model.getMean(action, state.mean()); // 2 Matrix q = model.a() * state.covariance() * model.a().Transpose(); //2a q += model.getError(action, state.mean()); // 2b - return std::make_shared>(std::move(x1), std::move(q)); + return std::make_shared > (std::move(x1), std::move(q)); } - std::shared_ptr> marginalize(const KalmanActionModel& model, - const GaussianMoment& action, - const GaussianMoment& state) const + std::shared_ptr> marginalize( + const KalmanActionModel& model, + const GaussianMoment & action, + const GaussianMoment & state + ) const { auto x1 = marginalize(model, action.mean(), state); const Matrix& A = model.a(); const Matrix& B = model.b(); - Matrix q = A * state.covariance() * A.transpose(); - q += (B * action.covariance() * B.transpose()); + Matrix q = A * state.covariance() * A.Transpose(); + q += (B * action.covariance() * B.Transpose()); q += x1->covariance(); - return std::make_shared>(x1->mean(), std::move(q)); + return std::make_shared > (x1->mean(), std::move(q)); } - }; } } } -#endif // KALMAN_FILTER_HPP \ No newline at end of file +#endif //KALMAN_FILTER_HPP diff --git a/core/navigation/pose/include/pose/matrix.hpp b/core/navigation/pose/include/pose/matrix.hpp new file mode 100644 index 0000000..faa38a8 --- /dev/null +++ b/core/navigation/pose/include/pose/matrix.hpp @@ -0,0 +1,74 @@ +#ifndef MATRIX_HPP +#define MATRIX_HPP + +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace pose + { + class Vector; + + class Matrix + { + public: + static Matrix identity(std::size_t order); + + Matrix(); + Matrix(std::size_t rows, std::size_t cols); + Matrix(std::size_t rows, std::size_t cols, std::vector values); + Matrix(std::size_t rows, std::size_t cols, std::vector values, bool row_based); + Matrix(const Matrix& rhs); + Matrix(Matrix&& rhs); + + virtual ~Matrix(); + + std::size_t rows() const; + std::size_t cols() const; + std::size_t order() const; + + double at(std::size_t row, std::size_t col) const; + Vector column(std::size_t col) const; + + Matrix& operator=(Matrix&& rhs); + Matrix& operator+=(const Matrix& rhs); + Matrix operator+(const Matrix& rhs) const; + Matrix operator-(const Matrix& rhs) const; + Matrix operator*(const Matrix& rhs) const; + + Matrix transpose() const; + Matrix inverse() const; + Matrix sqrt() const; + + double determinant() const; + void scaleRow(std::size_t r, double k); + void combineRow(std::size_t src_row, std::size_t dst_row, double k); + + Matrix LUDecompositionByGE(int* p_swap_count, std::vector< std::size_t >* p_pivot) const; + Matrix roundSymmetry(); + + private: + std::size_t getIndex(std::size_t row, std::size_t col) const; + static void initPivots(std::size_t n, std::vector* p_pivot); + double partialPivot(std::vector* p_pivot, std::size_t k, int* p_swap_count) const; + bool isSymmetric() const; + Matrix choleskyDecomposition() const; + + std::size_t m_rows; + std::size_t m_cols; + std::vector m_m; + bool m_row_based = true; + + friend Matrix operator*(double scalar, const Matrix& m); + }; + + std::ostream& operator<<(std::ostream& os, const Matrix& m); + } + } +} + +#endif //MATRIX_HPP diff --git a/core/navigation/pose/include/pose/matrix_vector_operators.hpp b/core/navigation/pose/include/pose/matrix_vector_operators.hpp new file mode 100644 index 0000000..908cd31 --- /dev/null +++ b/core/navigation/pose/include/pose/matrix_vector_operators.hpp @@ -0,0 +1,21 @@ +#ifndef MATRIX_VECTOR_OPERATORS_HPP +#define MATRIX_VECTOR_OPERATORS_HPP + +#include "pose/vector.hpp" +#include "pose/matrix.hpp" + +namespace rip +{ + namespace navigation + { + namespace pose + { + Vector operator*(const Matrix& m, const Vector& v); + Vector operator*(const Vector& v, const Matrix& m); + Matrix operator*(double scalar, const Matrix& m); + double operator*(const Vector& v1, const Vector& v2); + } + } +} + +#endif //MATRIX_VECTOR_OPERATORS_HPP \ No newline at end of file diff --git a/core/navigation/pose/include/pose/random_distribution.hpp b/core/navigation/pose/include/pose/random_distribution.hpp new file mode 100644 index 0000000..08be7b0 --- /dev/null +++ b/core/navigation/pose/include/pose/random_distribution.hpp @@ -0,0 +1,29 @@ +#ifndef RANDOM_DISTRIBUTION_HPP +#define RANDOM_DISTRIBUTION_HPP + +#include + +namespace rip +{ + namespace navigation + { + namespace pose + { + template + class RandomDistribution { + public: + RandomDistribution() { + } + + virtual ~RandomDistribution() { + } + + virtual double probabilityOf(const X& x) const = 0; + + virtual X sample(std::default_random_engine* generator) const = 0; + }; + } + } +} + +#endif //RANDOM_DISTRIBUTION_HPP diff --git a/core/navigation/localization/include/localization/sensor_model.hpp b/core/navigation/pose/include/pose/sensor_model.hpp similarity index 65% rename from core/navigation/localization/include/localization/sensor_model.hpp rename to core/navigation/pose/include/pose/sensor_model.hpp index c0ac473..1a9829a 100644 --- a/core/navigation/localization/include/localization/sensor_model.hpp +++ b/core/navigation/pose/include/pose/sensor_model.hpp @@ -5,14 +5,19 @@ namespace rip { namespace navigation { - namespace localization + namespace pose { template class SensorModel { public: - SensorModel() {} - virtual ~SensorModel() {} + SensorModel() + { + } + + virtual ~SensorModel() + { + } virtual double conditionalProbabilityOf(const Z& observation, const X& state) const = 0; }; @@ -20,4 +25,4 @@ namespace rip } } -#endif // SENSOR_MODEL_HPP \ No newline at end of file +#endif //SENSOR_MODEL_HPP diff --git a/core/navigation/pose/include/pose/unscented_kalman_filter.hpp b/core/navigation/pose/include/pose/unscented_kalman_filter.hpp new file mode 100644 index 0000000..70f18f1 --- /dev/null +++ b/core/navigation/pose/include/pose/unscented_kalman_filter.hpp @@ -0,0 +1,188 @@ +#ifndef UNSCENTED_KALMAN_FILTER_HPP +#define UNSCENTED_KALMAN_FILTER_HPP + +#include +#include + +#include "pose/gaussian_moment.hpp" +#include "pose/matrix.hpp" +#include "pose/vector.hpp" + +namespace rip +{ + namespace navigation + { + namespace pose + { + template + struct SigmaPoint + { + static std::vector> + createSigmaPoints(const GaussianMoment & belief, double alpha, double beta, double lambda) + { + + std::size_t order = belief.order(); + double n = static_cast(order); + + Matrix p0 = ((n + lambda) * belief.covariance()); + // we need to ensure that P0 is symmetric + Matrix p1 = p0.roundSymmetry(); + Matrix p = p1.sqrt().Transpose(); + + std::vector > sigma_pts(2 * order + 1); + + double wm0 = lambda / (lambda + n); + double wc0 = wm0 + (1 - (alpha * alpha) + beta); + double wn = 1 / (2 * (lambda + n)); + + sigma_pts[0] = SigmaPoint(belief.mean(), wm0, wc0); + for(std::size_t i = 0; i < order; i ++) + { + X2 x = belief.mean(); + Vector dx = p.column(i); + X2 x1; + x1.Set(x + dx); + X2 x2; + x2.Set(x - dx); + sigma_pts[i + 1] = SigmaPoint(x1, wn, wn); + sigma_pts[i + 1 + order] = SigmaPoint(x2, wn, wn); + } + return sigma_pts; + } + + static Y getWeightedMean(const std::vector >& sigma_pts, std::size_t n) + { + std::vector a(n); + for(std::size_t i = 0; i < sigma_pts.size(); i ++) + { + SigmaPoint sp = sigma_pts[i]; + for(std::size_t j = 0; j < a.size(); j ++) + { + a[j] += sp.mean_weight * sp.post[j]; + } + } + Vector y(std::move(a)); + return Y(std::move(y)); + } + + static Matrix getWeightedError(const std::vector >& sigma_pts, std::size_t n, Y next) + { + + Matrix next_e(n, n); + for(std::size_t i = 0; i < sigma_pts.size(); i ++) + { + SigmaPoint sp = sigma_pts[i]; + Vector dt = sp.post - next; + next_e = next_e + (sp.error_weight * dt.Cross(dt) + sp.error); + } + return next_e; + } + + X2 prior; + Y post; + Matrix error; + double mean_weight; + double error_weight; + + SigmaPoint(X2 p, double w1, double w2) : prior(p), mean_weight(w1), error_weight(w2) + { + } + + SigmaPoint() : prior(), mean_weight(0.0), error_weight(0.0) + { + } + }; + + template + class UnscentedKalmanFilter + { + public: + UnscentedKalmanFilter(double alpha, double beta, double kappa) : m_alpha(alpha), m_beta(beta), m_kappa(kappa) + { + } + + virtual ~UnscentedKalmanFilter() + { + } + + std::shared_ptr< GaussianMoment > marginalize(const GaussianActionModel & model, const U& action, const GaussianMoment & belief) + { + std::vector> sigma_x = SigmaPoint::createSigmaPoints(belief, m_alpha, m_beta, + lambda(belief)); + + for(std::size_t i = 0; i < sigma_x.size(); i ++) + { + sigma_x[i].post = model.getMean(action, sigma_x[i].prior); + sigma_x[i].error = model.getError(action, belief.mean()); + } + + std::size_t n = belief.order(); + X next_x = SigmaPoint::getWeightedMean(sigma_x, n); + Matrix next_e = SigmaPoint::getWeightedError(sigma_x, n, next_x); + + return std::make_shared > (next_x, next_e); + } + + std::shared_ptr>bayesianInference(const GaussianSensorModel & model, const Z& observation, const GaussianMoment & belief) + { + std::vector> sigma_z = SigmaPoint::createSigmaPoints(belief, m_alpha, m_beta, + lambda(belief)); + + for(std::size_t i = 0; i < sigma_z.size(); i ++) + { + sigma_z[i].post = model.getMean(sigma_z[i].prior); + sigma_z[i].error = model.getError(sigma_z[i].post); + } + + std::size_t n = belief.order(); + Z next_z = SigmaPoint::getWeightedMean(sigma_z, n); + Matrix S = SigmaPoint::getWeightedError(sigma_z, n, next_z); + + Matrix E(n, n); + for(std::size_t i = 0; i < sigma_z.size(); i ++) + { + SigmaPoint sp = sigma_z[i]; + Vector dx = sigma_z[i].prior - belief.mean(); + Vector du = sigma_z[i].post - next_z; + E = E + sp.error_weight * dx.cross(du); + } + + Matrix K = E * S.inverse(); + + X next_x(belief.mean() + K * (observation - next_z)); + Matrix next_e = belief.covariance() - K * S * K.transpose(); + + return std::make_shared > (next_x, next_e); + } + + private: + double lambda(const GaussianMoment & belief) const + { + double n = static_cast(belief.order()); + return (m_alpha * m_alpha) * (n + m_kappa) - n; + } + + /** + * Primary scaling factor and determines the spread of the Sigma points + * around the x mean and is usually set to a small positive value (e.g.1e-3) + */ + double m_alpha; + + /** + * Beta is the secondary scaling factor and determines the weight given to + * the mean when recombing the sigma point. + * Beta incorporates additional higher order knowledge about the + * distribution Beta=2 indicates the distribution is Gaussian. + */ + double m_beta; + + /** + * Kappa is a tertiary scaling parameter and is usually set to zero + */ + double m_kappa; + }; + } + } +} + +#endif //UNSCENTED_KALMAN_FILTER_HPP diff --git a/core/navigation/pose/include/pose/vector.hpp b/core/navigation/pose/include/pose/vector.hpp new file mode 100644 index 0000000..7dae0ee --- /dev/null +++ b/core/navigation/pose/include/pose/vector.hpp @@ -0,0 +1,85 @@ +#ifndef VECTOR_HPP +#define VECTOR_HPP + +#include +#include + +#include "matrix.hpp" + +namespace rip +{ + namespace navigation + { + namespace pose + { + class Vector + { + public: + Vector(); + + explicit Vector(std::vector a); + + explicit Vector(const Vector& v); + + Vector(Vector&& v); + + explicit Vector(std::size_t order); + + virtual ~Vector(); + + std::size_t order() const; + + double& at(int i); + + double at(int i) const; + + double& operator[](std::size_t i); + + double operator[](std::size_t i) const; + + Matrix cross(const Vector& rhs) const; + + Vector& operator=(const Vector& rhs); + + virtual void set(Vector a); + + void copyAssign(const Vector& v); + + void moveAssign(Vector&& v); + + Vector aliasVector() const; + + double dot(const Vector& v2) const; + + Vector operator+(const Vector& rhs) const; + + Vector operator-(const Vector& rhs) const; + + bool operator<(const Vector& rhs) const; + + bool operator>(const Vector& rhs) const; + + bool operator==(const Vector& rhs) const; + + private: + std::vector m_a; + }; + + std::ostream& operator<<(std::ostream& os, const Vector& v); + + +#define DEFINE_VECTOR1(X) \ +class X : public rip::navigation::pose::Vector { \ +public: \ + X() : Vector(std::vector({ 0.0 })) {} \ + X(double x) : Vector(std::vector({ x })) {} \ + X(const X& v) : Vector(v) {} \ + X(const X&& v) : Vector(v) {} \ + X(rip::navigation::pose::Vector&& v) : Vector(v) {} \ + X& operator=(const X& that) { Vector::operator=(that); return *this; } \ +} + } + } +} + +#endif //VECTOR_HPP diff --git a/core/navigation/pose/include/pose/vector_range.hpp b/core/navigation/pose/include/pose/vector_range.hpp new file mode 100644 index 0000000..6cb8749 --- /dev/null +++ b/core/navigation/pose/include/pose/vector_range.hpp @@ -0,0 +1,61 @@ +#ifndef VECTOR_RANGE_HPP +#define VECTOR_RANGE_HPP + +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace pose + { + template + class VectorRange + { + public: + VectorRange(X min, X max) : m_min(std::move(min)), m_max(std::move(max)) + { + } + + virtual ~VectorRange() + { + } + + double area() const + { + Vector dx = m_max - m_min; + double result = 1.0; + for(unsigned int i = 0; i < dx.order(); i ++) + { + result *= dx[i]; + } + return fabs(result); + } + + bool contains(const X& x) const + { + return x > m_min && x < m_max; + } + + X sample(std::default_random_engine* generator) const + { + X result; + for(unsigned int i = 0; i < result.order(); i ++) + { + std::uniform_real_distribution random(m_min[i], m_max[i]); + result[i] = random(*generator); + } + return result; + } + + private: + X m_min; + X m_max; + }; + } + } +} + +#endif //VECTOR_RANGE_HPP diff --git a/core/navigation/pose/src/matrix.cpp b/core/navigation/pose/src/matrix.cpp new file mode 100644 index 0000000..5b917ca --- /dev/null +++ b/core/navigation/pose/src/matrix.cpp @@ -0,0 +1,499 @@ +#include "pose/matrix.hpp" +#include "pose/vector.hpp" +#include + +namespace rip +{ + namespace navigation + { + namespace pose + { + Matrix::Matrix() : m_rows(0), m_cols(0), m_m(), m_row_based(true) + { + } + + Matrix::Matrix(std::size_t rows, std::size_t cols) : m_rows(rows), m_cols(cols), m_m(rows * cols), + m_row_based(true) + { + } + + Matrix::Matrix(std::size_t rows, std::size_t cols, std::vector values) : m_rows(rows), m_cols(cols), + m_m(values), + m_row_based(true) + { + } + + Matrix::Matrix(std::size_t rows, std::size_t cols, std::vector values, bool row_based) : m_rows( + rows), m_cols(cols), m_m(values), m_row_based(row_based) + { + } + + Matrix::Matrix(const Matrix& that) : m_rows(that.m_rows), m_cols(that.m_cols), m_m(that.m_m), + m_row_based(that.m_row_based) + { + } + + Matrix::Matrix(Matrix&& that) : m_rows(that.m_rows), m_cols(that.m_cols), m_m(std::move(that.m_m)), + m_row_based(that.m_row_based) + { + } + + Matrix::~Matrix() + { + } + + std::size_t Matrix::rows() const + { + return m_rows; + } + + std::size_t Matrix::cols() const + { + return m_cols; + } + + std::size_t Matrix::order() const + { + if(m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + return m_rows; + } + + double Matrix::at(std::size_t row, std::size_t col) const + { + return m_m[getIndex(row, col)]; + } + + Vector Matrix::column(std::size_t col) const + { + Vector result; + std::vector a(m_rows); + if(m_row_based) + { + for(std::size_t i = 0; i < a.size(); i ++) + { + a[i] = at(i, col); + } + }else + { + std::size_t col_start = getIndex(0, col); + std::size_t col_end = col_start + a.size(); + std::copy(m_m.begin() + col_start, m_m.begin() + col_end, a.begin()); + } + return Vector(a); + } + + Matrix& Matrix::operator=(Matrix&& that) + { + m_rows = that.m_rows; + m_cols = that.m_cols; + m_m = std::move(that.m_m); + m_row_based = that.m_row_based; + return *this; + } + + Matrix& Matrix::operator+=(const Matrix& that) + { + if(m_rows != that.m_rows || m_cols != that.m_cols) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + + if(m_row_based == that.m_row_based) + { + for(std::size_t i = 0; i < m_m.size(); i ++) + { + m_m[i] += that.m_m[i]; + } + }else + { + for(std::size_t row = 0; row < rows(); row ++) + { + for(std::size_t col = 0; col < cols(); col ++) + { + std::size_t this_index = getIndex(row, col); + std::size_t that_index = that.getIndex(row, col); + m_m[this_index] += that.m_m[that_index]; + } + } + } + + return *this; + } + + Matrix Matrix::operator+(const Matrix& that) const + { + if(m_rows != that.m_rows || m_cols != that.m_cols || m_row_based != that.m_row_based) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + std::vector m(m_m.size()); + for(std::size_t i = 0; i < m_m.size(); i ++) + { + m[i] = m_m[i] + that.m_m[i]; + } + return Matrix(m_rows, m_cols, m, m_row_based); + } + + Matrix Matrix::operator-(const Matrix& that) const + { + if(m_rows != that.m_rows || m_cols != that.m_cols || m_row_based != that.m_row_based) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + std::vector m(m_m.size()); + for(std::size_t i = 0; i < m_m.size(); i ++) + { + m[i] = m_m[i] - that.m_m[i]; + } + return Matrix(m_rows, m_cols, m, m_row_based); + } + + Matrix Matrix::operator*(const Matrix& that) const + { + if(m_cols != that.m_rows) + { + throw std::runtime_error("Matrix sizes not compatible."); + } + Matrix result(m_rows, that.m_cols); + for(std::size_t col = 0; col < that.m_cols; col ++) + { + for(std::size_t row = 0; row < m_rows; row ++) + { + for(std::size_t k = 0; k < m_cols; k ++) + { + result.m_m[result.getIndex(row, col)] += at(row, k) * that.at(k, col); + } + } + } + return result; + } + + Matrix Matrix::transpose() const + { + return Matrix(m_cols, m_rows, m_m, ! m_row_based); + } + + Matrix Matrix::inverse() const + { + if(m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + // set up mean as identity + Matrix result(m_rows, m_cols); + for(std::size_t i = 0; i < m_cols; i ++) + { + result.m_m[result.getIndex(i, i)] = 1.0; + } + + Matrix working(m_rows, m_cols, m_m); // creates a copy of m_ + // Eliminate L + for(std::size_t col = 0; col < m_cols; col ++) + { + double diag = working.at(col, col); + for(std::size_t row = col + 1; row < m_rows; row ++) + { + double target = working.at(row, col); + double a = - target / diag; + working.combineRow(col, row, a); + result.combineRow(col, row, a); + } + working.scaleRow(col, 1.0 / diag); + result.scaleRow(col, 1.0 / diag); + } + // Eliminate U + for(std::size_t col = m_cols - 1; col >= 1; col --) + { + double diag = working.at(col, col); // 1.0 + for(std::size_t row_plus_one = col; row_plus_one > 0; row_plus_one --) + { + std::size_t row = row_plus_one - 1; + double target = working.at(row, col); + double a = - target / diag; + working.combineRow(col, row, a); + result.combineRow(col, row, a); + } + } + return result; + } + + Matrix Matrix::sqrt() const + { + // The matrix square root should be calculated using numerically efficient and stable methods such as the Cholesky decomposition + if(m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + return choleskyDecomposition(); + } + + double Matrix::determinant() const + { + if(rows() != cols()) + { + throw std::runtime_error("Matrix must be square."); + } + switch(rows()) + { + case 0: + return 1; + case 1: + return m_m[0]; + case 2: + { + return m_m[0] * m_m[3] - m_m[1] * m_m[2]; + } + case 3: + { + double a = at(0, 0); + double b = at(0, 1); + double c = at(0, 2); + double d = at(1, 0); + double e = at(1, 1); + double f = at(1, 2); + double g = at(2, 0); + double h = at(2, 1); + double i = at(2, 2); + return a * e * i + b * f * g + c * d * h - a * f * h - b * d * i - c * e * g; + } + default: + { + int swapCount; + std::vector pivot; + Matrix LU = LUDecompositionByGE(&swapCount, &pivot); + double result = 1.0; + for(std::size_t k = 0; k < LU.order(); k ++) + { + result *= LU.at(pivot[k], k); + } + if(swapCount % 2 == 1) + { + result = - result; + } + return result; + } + } + } + + void Matrix::scaleRow(std::size_t r, double k) + { + for(std::size_t c = 0; c < m_cols; c ++) + { + m_m[getIndex(r, c)] = k * at(r, c); + } + } + + void Matrix::combineRow(std::size_t srcRow, std::size_t dstRow, double k) + { + for(std::size_t c = 0; c < m_cols; c ++) + { + m_m[getIndex(dstRow, c)] = at(dstRow, c) + k * at(srcRow, c); + } + } + + Matrix Matrix::LUDecompositionByGE(int* p_swap_count, std::vector* p_pivot) const + { + if(m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + std::vector& pivot = *p_pivot; + *p_swap_count = 0; + std::size_t n = rows(); + + // Make a copy of the matrix. + // We never change the original matrix because the backing array may be reused. + std::vector a(m_m); + Matrix m(n, n, a); + + initPivots(n, p_pivot); + for(std::size_t k = 0; k < n - 1; k ++) + { + double maxValue = m.partialPivot(p_pivot, k, p_swap_count); + if(maxValue == 0) + { + throw std::runtime_error("Matrix is singular."); + } + double m_kk = m.at(pivot[k], k); + for(std::size_t i = k + 1; i < n; i ++) + { + std::size_t ik = m.getIndex(pivot[i], k); + m.m_m[ik] /= m_kk; + } + for(std::size_t i = k + 1; i < n; i ++) + { + double m_ik = m.at(pivot[i], k); + for(std::size_t j = k + 1; j < n; j ++) + { + double m_kj = m.at(pivot[k], j); + std::size_t ij = m.getIndex(pivot[i], j); + m.m_m[ij] -= m_ik * m_kj; + } + } + } + if(m.at(pivot[n - 1], n - 1) == 0) + { + throw std::runtime_error("Matrix is singular."); + } + return m; + } + + Matrix Matrix::roundSymmetry() + { + std::vector a(m_m.size()); + for(std::size_t i = 0; i < m_rows; i ++) + { + std::size_t diagIndex = getIndex(i, i); + a[diagIndex] = m_m[diagIndex]; + for(std::size_t j = i + 1; j < m_cols; j ++) + { + std::size_t i1 = getIndex(i, j); + std::size_t i2 = getIndex(j, i); + double mean = (m_m[i1] + m_m[i2]) / 2; + a[i1] = a[i2] = mean; + } + } + return Matrix(m_rows, m_cols, a); + } + + std::size_t Matrix::getIndex(std::size_t row, std::size_t col) const + { + if(m_row_based) + return row * m_cols + col; + else + return col * m_rows + row; + } + + /* static */ + void Matrix::initPivots(std::size_t n, std::vector* p_pivot) + { + std::vector& pivot = *p_pivot; + pivot.resize(n); + for(std::size_t i = 0; i < pivot.size(); i ++) + { + pivot[i] = i; + } + } + + double Matrix::partialPivot(std::vector* p_pivot, std::size_t k, int* p_swap_count) const + { + std::vector& pivot = *p_pivot; + int& swap_count = *p_swap_count; + double maxValue = fabs(at(pivot[k], k)); + std::size_t maxIndex = k; + for(std::size_t i = k + 1; i < pivot.size(); i ++) + { + std::size_t rowIndex = pivot[i]; + double rowValue = fabs(at(rowIndex, k)); + if(rowValue > maxValue) + { + maxValue = rowValue; + maxIndex = i; + } + } + if(maxIndex != k) + { + std::size_t temp = pivot[maxIndex]; + pivot[maxIndex] = pivot[k]; + pivot[k] = temp; + swap_count ++; + } + return maxValue; + } + + bool Matrix::isSymmetric() const + { + if(m_rows != m_cols) + { + return false; + } + for(std::size_t i = 1; i < rows(); i ++) + { + for(std::size_t j = 1; j < i; j ++) + { + if(at(i, j) != at(j, i)) + { + return false; + } + } + } + return true; + } + + Matrix Matrix::choleskyDecomposition() const + { + // returns an upper diagonal matrix + if(m_rows != m_cols) + { + throw std::runtime_error("Matrix must be square."); + } + if(! isSymmetric()) + { + throw std::runtime_error("Matrix must be symmetric."); + } + std::size_t n = rows(); + std::vector a(m_m); + for(std::size_t i = 0; i < n; i ++) + { + std::size_t ii = getIndex(i, i); + for(std::size_t k = 0; k < i; k ++) + { + std::size_t ki = getIndex(k, i); + a[ii] = a[ii] - a[ki] * a[ki]; + } + if(a[ii] < 0) + { + throw std::runtime_error("Matrix is not positive definite."); + } + a[ii] = std::sqrt(a[ii]); + for(std::size_t j = i + 1; j < n; j ++) + { + std::size_t ij = getIndex(i, j); + for(std::size_t k = 0; k < i; k ++) + { + std::size_t ki = getIndex(k, i); + std::size_t kj = getIndex(k, j); + a[ij] = a[ij] - a[ki] * a[kj]; + } + if(a[ij] != 0) + a[ij] = a[ij] / a[ii]; + } + } + // Clear out the lower matrix + for(std::size_t i = 1; i < n; i ++) + { + for(std::size_t j = 0; j < i; j ++) + { + std::size_t ij = getIndex(i, j); + a[ij] = 0; + } + } + return Matrix(n, n, a); + } + + std::ostream& operator<<(std::ostream& os, const Matrix& m) + { + os << m.rows() << "x" << m.cols() << ":{"; + for(std::size_t row = 0; row < m.rows(); row ++) + { + if(row > 0) + os << ", "; + os << "{"; + for(std::size_t col = 0; col < m.cols(); col ++) + { + if(col > 0) + os << ", "; + os << m.at(row, col); + } + os << "}"; + } + os << "}"; + return os; + + } + } + } +} \ No newline at end of file diff --git a/core/navigation/pose/src/matrix_vector_operators.cpp b/core/navigation/pose/src/matrix_vector_operators.cpp new file mode 100644 index 0000000..a1def9c --- /dev/null +++ b/core/navigation/pose/src/matrix_vector_operators.cpp @@ -0,0 +1,69 @@ +#include "pose/matrix_vector_operators.hpp" + +#include + +namespace rip +{ + namespace navigation + { + namespace pose + { + + Vector operator*(const Matrix& m, const Vector& v) + { + if(m.cols() != v.order()) + { + std::stringstream error_message; + error_message << "Matrix and Vector sizes not compatible " << "(" << m << " * " << v << ")"; + throw std::runtime_error(error_message.str()); + } + std::vector result(m.rows()); + for(std::size_t row = 0; row < m.rows(); row ++) + { + double value = 0.0; + for(std::size_t col = 0; col < m.cols(); col ++) + { + value += m.at(row, col) * v[col]; + } + result[row] = value; + } + return Vector(result); + } + + Vector operator*(const Vector& v, const Matrix& m) + { + if(v.order() != m.rows()) + { + throw std::runtime_error("Matrix size is not compatible with Vector size."); + } + std::vector result(v.order()); + for(std::size_t col = 0; col < m.cols(); col ++) + { + for(std::size_t row = 0; row < m.rows(); row ++) + { + result[row] += v[row] * m.at(row, col); + } + } + return Vector(result); + + } + + Matrix operator*(double scalar, const Matrix& m) + { + const std::vector& ma = m.m_m; + std::vector a(ma.size()); + for(std::size_t i = 0; i < a.size(); i ++) + { + a[i] = ma[i] * scalar; + } + return Matrix(m.rows(), m.cols(), a); + + } + + double operator*(const Vector& v1, const Vector& v2) + { + return v1.dot(v2); + } + } + } +} \ No newline at end of file diff --git a/core/navigation/pose/src/vector.cpp b/core/navigation/pose/src/vector.cpp new file mode 100644 index 0000000..240bd57 --- /dev/null +++ b/core/navigation/pose/src/vector.cpp @@ -0,0 +1,205 @@ +#include "pose/vector.hpp" + +namespace rip +{ + namespace navigation + { + namespace pose + { + Vector::Vector() + { + } + + Vector::Vector(std::vector a) : m_a(std::move(a)) + { + } + + Vector::Vector(const Vector& v) : m_a(v.m_a) + { + } + + Vector::Vector(Vector&& v) : m_a(std::move(v.m_a)) + { + } + + Vector::Vector(std::size_t order) : m_a(order) + { + } + + Vector::~Vector() + { + } + + double Vector::dot(const Vector& v2) const + { + const Vector& v1 = *this; + double result = 0.0; + for(std::size_t i = 0; i < v1.order(); i ++) + { + result += v1.m_a[i] * v2.m_a[i]; + } + return result; + + } + + std::size_t Vector::order() const + { + return m_a.size(); + } + + void Vector::set(Vector a) + { + if(order() != a.order()) + { + throw std::runtime_error("Length of array is different from the Vector expected order."); + } + m_a = std::move(a.m_a); + } + + double& Vector::at(int i) + { + return m_a[i]; + } + + double Vector::at(int i) const + { + return m_a[i]; + } + + Vector Vector::operator+(const Vector& v2) const + { + const Vector& v1 = *this; + if(v1.order() != v2.order()) + { + throw std::runtime_error("Vector sizes are not compatible."); + } + Vector result(std::vector(v1.order())); + std::vector& a = result.m_a; + for(std::size_t i = 0; i < a.size(); i ++) + { + a[i] = v1.m_a[i] + v2.m_a[i]; + } + return result; + } + + Vector Vector::operator-(const Vector& v2) const + { + const Vector& v1 = *this; + if(v1.order() != v2.order()) + { + throw std::runtime_error("Vector sizes are not compatible."); + } + Vector result(v1.order()); + std::vector& a = result.m_a; + for(std::size_t i = 0; i < a.size(); i ++) + { + a[i] = v1.m_a[i] - v2.m_a[i]; + } + return result; + } + + bool Vector::operator<(const Vector& v2) const + { + if(this == &v2) + return false; + const Vector& v1 = *this; + bool result = v1.order() > 0; + for(std::size_t i = 0; i < v1.order(); i ++) + { + result = result && v1.m_a[i] < v2.m_a[i]; + } + return result; + } + + bool Vector::operator>(const Vector& v2) const + { + if(this == &v2) + return false; + const Vector& v1 = *this; + bool result = v1.order() > 0; + for(std::size_t i = 0; i < v1.order(); i ++) + { + result = result && v1.m_a[i] > v2.m_a[i]; + } + return result; + } + + bool Vector::operator==(const Vector& v) const + { + if(this == &v) + return true; + return m_a == v.m_a; + } + + Vector& Vector::operator=(const Vector& that) + { + this->m_a = that.m_a; + return *this; + } + + // operator= is implicitly declared as deleted in subclasses + // because it also defines a move assignment operator. + void Vector::copyAssign(const Vector& v) + { + if(this != &v) + { + m_a = v.m_a; + } + } + + void Vector::moveAssign(Vector&& v) + { + if(this != &v) + { + m_a = std::move(v.m_a); + } + } + + double& Vector::operator[](std::size_t i) + { + return m_a[i]; + } + + double Vector::operator[](std::size_t i) const + { + return m_a[i]; + } + + Matrix Vector::cross(const Vector& that) const + { + std::vector m(order() * that.order()); + int k = 0; + for(std::size_t i = 0; i < order(); i ++) + { + double a = m_a[i]; + for(std::size_t j = 0; j < that.order(); j ++) + { + double b = that[j]; + m[k] = a * b; + k ++; + } + } + return Matrix(order(), that.order(), m); + } + + Vector Vector::aliasVector() const + { + return Vector(m_a); + } + + std::ostream& operator<<(std::ostream& os, const Vector& v) + { + os << v.order() << ":{"; + for(std::size_t i = 0; i < v.order(); i ++) + { + if(i > 0) + os << ", "; + os << v[i]; + } + os << "}"; + return os; + } + + } + } +} \ No newline at end of file From 720e845a786f038ee08aad32da4bb556c902a451 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 27 Mar 2018 20:38:01 -0400 Subject: [PATCH 079/200] progress on xml (creds ben J), cpp and hpp --- appendages/include/appendages/roboclaw.hpp | 41 +-- appendages/src/roboclaw.cpp | 248 ++++++++++++-- appendages/xml/roboclaw.xml | 367 +++++++++++++++------ 3 files changed, 500 insertions(+), 156 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 6e2b00b..1bfa419 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -23,8 +23,8 @@ namespace rip void setM1M2Speed(int32_t speed); void setM1SpeedAccel(uint32_t accel, int32_t speed); - void setM2SpeedAccel(int32_t accel, int32_t speed); - void setM1M2SpeedAccel(int32_t accel, int32_t speed1, int32_t speed2); + void setM2SpeedAccel(uint32_t accel, int32_t speed); + void setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2); void setM1SpeedDist(int32_t speed, uint32_t distance); void setM2SpeedDist(int32_t speed, uint32_t distance); @@ -36,31 +36,32 @@ namespace rip void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); - void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); + void setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, + uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2); int32_t readM1Encoder(); int32_t readM2Encoder(); std::array readM1M2Encoders(); - std::array readEncoders(); - units::Distance readEncoder(bool motor); + std::array readEncoders(); //Not implemented on arduino + units::Distance readEncoder(bool motor); //Not implemented on arduino int32_t readM1EncoderSpeed(); int32_t readM2EncoderSpeed(); - int32_t readM1M2EncoderSpeed(); - std::array readEncoderSpeeds(); - units::Velocity readEncoderSpeed(bool motor); + std::array readM1M2EncoderSpeed(); + std::array readEncoderSpeeds(); //Not implemented on arduino + units::Velocity readEncoderSpeed(bool motor); //Not implemented on arduino void setM1Duty(int16_t duty); void setM2Duty(int16_t duty); - void setDuties(int16_t duty1, int16_t duty2); - void setDuty(bool motor, int16_t duty); - + void setDuties(int16_t duty1, int16_t duty2); //Not implemented on arduino + void setDuty(bool motor, int16_t duty); //Not implemented on arduino + void setVelocityPID(bool motor, float Kp, float Ki, float Kd, uint32_t qpps); std::array getBuffers(); void resetEncoders(); - //void setSpeeds(int32_t speed1, uint32_t speed2); - void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); - void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); + + void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); //Not implemented on arduino + void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); //Not implemented on arduino /** * Stop! ^0^ */ @@ -128,13 +129,13 @@ namespace rip std::shared_ptr m_set_m2_duty; std::shared_ptr m_set_m1m2_duty; - std::shared_ptr m_set_velocity_pid; + std::shared_ptr m_set_speed_pid; - std::shared_ptr m_read_m1_encoder_velocity; - std::shared_ptr m_read_m2_encoder_velocity; - std::shared_ptr m_read_m1m2_encoder_velocity; - std::shared_ptr m_read_encoder_velocity_result; - std::shared_ptr m_read_encoders_velocity_result; + std::shared_ptr m_read_m1_encoder_speed; + std::shared_ptr m_read_m2_encoder_speed; + std::shared_ptr m_read_m1m2_encoder_speed; + std::shared_ptr m_read_encoder_speed_result; + std::shared_ptr m_read_encoders_speed_result; }; } diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index ace9990..5cfdde4 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -15,17 +15,104 @@ namespace rip { Roboclaw::Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device), - m_set_speed(createCommand("kSetSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_speed_dist(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - - m_read_encoders(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoder_speeds(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoders_speeds_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_duty(createCommand("kSetDuty", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + m_set_m1_speed(createCommand("kSetM1Speed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed(createCommand("kSetM2Speed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed(createCommand("kSetM1M2Speed", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_accel(createCommand("kSetM1SpeedAccel", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_accel(createCommand("kSetM2SpeedAccel", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel(createCommand("kSetM1M2SpeedAccel", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_dist(createCommand("kSetM1SpeedDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_dist(createCommand("kSetM2SpeedDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_dist(createCommand("kSetM1M2SpeedDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_speed_accel_dist(createCommand("kSetM1SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_accel_dist(createCommand("kSetM2SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel_dist(createCommand("kSetM1M2SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel_decel_dist(createCommand("kSetM1M2SpeedAccelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1_encoder(createCommand("kReadM1Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder(createCommand("kReadM2Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder(createCommand("kReadM1M2Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoder_result(createCommand("kReadEncoderResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoders_result(createCommand("kReadEncodersResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1_encoder_speed(createCommand("kReadM1Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder_speed(createCommand("kReadM2Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder_speed(createCommand("kReadM1M2Encoder", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoder_speed_result(createCommand("kReadEncoderSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_encoders_speed_result(createCommand("kReadEncodersSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1_duty(createCommand("kSetM1Duty", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_duty(createCommand("kSetM2Duty", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_duty(createCommand("kSetM1M2Duty", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) { if (config.find("address") == config.end()) { @@ -50,16 +137,131 @@ namespace rip return std::dynamic_pointer_cast(std::shared_ptr(new Roboclaw(config, command_map, device))); } + void Roboclaw::setM1Speed(int32_t speed) + { + + } + + void Roboclaw::setM2Speed(int32_t speed) + { + + } + + void Roboclaw::setM1M2Speed(int32_t speed) + { + + } + + void Roboclaw::setM1SpeedAccel(uint32_t accel, int32_t speed) + { + + } + + void Roboclaw::setM2SpeedAccel(int32_t accel, int32_t speed) + { + + } + + void Roboclaw::setM1M2SpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) + { + + } + + void Roboclaw::setM1SpeedDist(int32_t speed, uint32_t distance) + { + + } + + void Roboclaw::setM2SpeedDist(int32_t speed, uint32_t distance) + { + + } + + void Roboclaw::setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) + { + + } + + void Roboclaw::setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) + { + + } + + void Roboclaw::setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) + { + + } + + void Roboclaw::setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) + { + + } + + void Roboclaw::setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) + { + + } + + void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) + { + + } + + void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) + { + + } + + int32_t Roboclaw::readM1Encoder() + { + + } + + int32_t Roboclaw::readM2Encoder() + { + + } + + std::array Roboclaw::readM1M2Encoders() + { + + } + units::Distance Roboclaw::readEncoder(bool motor) { cmdmessenger::ArduinoCmdMessenger messenger; } - units::Velocity Roboclaw::readEncoderSpeed(bool motor) + int32_t Roboclaw::readM1EncoderSpeed() { - cmdmessenger::ArduinoCmdMessenger messenger; + + } + + int32_t Roboclaw::readM2EncoderSpeed() + { + + } + + std::array Roboclaw::readM1M2EncoderSpeed() + { + + } + + std::array Roboclaw::readEncoderSpeeds() + { + + } + + void Roboclaw::resetEncoders() + { + } + units::Velocity Roboclaw::readEncoderSpeed(bool motor) + { + + } + /* void Roboclaw::setSpeed(int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; @@ -78,11 +280,7 @@ namespace rip messenger.send(m_device, m_read_encoders, m_address); return messenger.receive(m_read_encoders_result); } - - std::array Roboclaw::readEncoderSpeeds() - { - cmdmessenger::ArduinoCmdMessenger messenger; - } + */ void Roboclaw::setDynamic(bool motor, const MotorDynamics& dynamics, bool respectBuffer) { @@ -99,21 +297,13 @@ namespace rip case MotorDynamics::DType::kSpeed: { speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); - messenger.send( - m_device, m_set_speed, m_address, speed1, speed2); } case MotorDynamics::DType::kSpeedAccel: { speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); accel = static_cast((*dynamics.getAcceleration() / (m_wheel_radius * M_PI * 2)).to(1 / (units::s * units::s)) * m_ticks_per_rev); - messenger.send( - m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + } case MotorDynamics::DType::kSpeedDist: { @@ -181,12 +371,12 @@ namespace rip { cmdmessenger::ArduinoCmdMessenger messenger; } - + /* void Roboclaw::setDuty(int16_t duty1, int16_t duty2) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_duty, m_address, duty1, duty2); - } + }*/ void Roboclaw::setVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) { diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 63056b8..1c1a396 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -12,115 +12,268 @@ roboclaws[$i$].begin($baudrate$); - - - - - - if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) // TODO: Check return value + + + + + + if (!roboclaws[indexNum].SpeedM1(address, speed)) // TODO: Check return value { - cmdMessenger.sendBinCmd(kError, kSetSpeed); - } - - - - - - - - - if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) // TODO: Check return value - { - cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); - } - - - in - - - - - - if (!roboclaws[indexNum].SpeedDistM1M2(address, accel, speed1, speed2)) // TODO: Check return value - { - cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); - } - - - - - - - - - if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) // TODO: Check return value - { - cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); - } - - - - - - - - - if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) // TODO: Check return value - { - cmdMessenger.sendBinCmd(kError, kSetSpeedAccel); - } - - - - - - - - if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) // TODO: Check return value - { - cmdMessenger.sendBinCmd(kError, kReadEncoders); - } - - - - - - - - if (!roboclaws[indexNum].DutyM1M2(address, duty1, duty2)) // TODO: Check return value - { - cmdMessenger.sendBinCmd(kError, kSetDuty); - } - - - - - - - - - - - bool result; + cmdMessenger.sendBinCmd(kError, kSetM1Speed); + } + + + + + + + if (!roboclaws[indexNum].SpeedM2(address, speed)) { + cmdMessenger.sendBinCmd(kerror, kSetM2Speed); + } + + + + + + + + if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) { + cmdMessenger.sendBinCmd(kerror, kSetM1M2Speed); + } + + - switch (motor) - { - case 1: - result = roboclaws[indexNum].SetM1VelocityPID(address, Kp, Ki, Kd, qpps); - break; - case 2: - result = roboclaws[indexNum].SetM2VelocityPID(address, Kp, Ki, Kd, qpps); - break; - default: - result = false; - break; - } + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1(address, accel, speed)) { + cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccel); + } + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM2(address, accel, speed)) { + cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccel); + } + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) { + cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedAccel); + } + + + + + + + + + + if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance)) { + cmdMessenger.sendBinCmd(kerror, kSetM1SpeedDist); + } + + + + + + + + if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance)) { + cmdMessenger.sendBinCmd(kerror, kSetM2SpeedDist); + } + + + + + + + + + + if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2)) { + cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedDist); + } + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance)) { + cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccelDist); + } + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance)) { + cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDist); + } + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2)) { + cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedAccelDist); + } + + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position)) { + cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccelDecelDist); + } + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position)) { + cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDecelDist); + } + + + + + + + + + + + + + + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2)) { + cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDecelDist); + } + + + + + + + + + + + + + + + + + + + + + + + + if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) { + cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); + } + + + + + + + + + + + + + + + bool RoboClaw::ReadEncoders(uint8_t address,uint32_t &enc1,uint32_t &enc2){ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - if (!result) // TODO: Double check return values - { - cmdMessenger.sendBinCmd(kError, kSetVelocityPID); - } - - From 40bdeefd1997faffe45a26608d360c347a53b33b Mon Sep 17 00:00:00 2001 From: xxAtrain223 Date: Tue, 27 Mar 2018 19:47:39 -0500 Subject: [PATCH 080/200] Changed the appendage cache to use a map of strings, json objects. --- arduino_gen/include/arduino_gen/appendage.hpp | 2 +- arduino_gen/src/appendage.cpp | 73 +++++++++---------- 2 files changed, 37 insertions(+), 38 deletions(-) diff --git a/arduino_gen/include/arduino_gen/appendage.hpp b/arduino_gen/include/arduino_gen/appendage.hpp index 07460b3..c1f64cb 100644 --- a/arduino_gen/include/arduino_gen/appendage.hpp +++ b/arduino_gen/include/arduino_gen/appendage.hpp @@ -94,7 +94,7 @@ namespace rip nlohmann::json m_data; - static std::map< std::string, std::map< std::string, std::string > > m_type_cache; + static std::map< std::string, nlohmann::json > m_type_cache; std::string m_appendage_data_folder; diff --git a/arduino_gen/src/appendage.cpp b/arduino_gen/src/appendage.cpp index ea7d80f..b291952 100644 --- a/arduino_gen/src/appendage.cpp +++ b/arduino_gen/src/appendage.cpp @@ -11,8 +11,7 @@ namespace rip { namespace arduinogen { - std::map< std::string, std::map< std::string, std::string> > Appendage::m_type_cache = - std::map< std::string, std::map< std::string, std::string> >(); + std::map< std::string, nlohmann::json > Appendage::m_type_cache = std::map< std::string, nlohmann::json >(); Appendage::Appendage(nlohmann::json j, std::multimap< std::string, std::shared_ptr >& appendages, @@ -149,83 +148,83 @@ namespace rip nlohmann::json j; (*type_template.createInputStream()) >> j; - std::map< std::string, std::string > temp; - for (nlohmann::json::iterator it = j.begin(); it != j.end(); ++it) { - if (it.key() == "core") - { - for (std::string core_key : it.value()) - { - if (m_data.find(core_key) == m_data.end()) - { - throw AppendageDataException(fmt::format("{} missing core field {}", label, core_key)); - } - m_core_fields.emplace_back(core_key); - } - } - else - { - temp[it.key()] = it.value(); - } - } - m_type_cache[type] = temp; + m_type_cache[type] = j; } - std::map< std::string, std::string >& type_check = m_type_cache[type]; + nlohmann::json& type_check = m_type_cache[type]; // Check if the appendage has all the parameters specified by the tempate // and that they are the correct type - for(std::pair< std::string, std::string> type_parameter : type_check) + for (nlohmann::json::iterator type_parameter = type_check.begin(); type_parameter != type_check.end(); ++type_parameter) { - if(m_data.find(type_parameter.first) == m_data.end()) + if (type_parameter.key() != "core" && m_data.find(type_parameter.key()) == m_data.end()) { - throw AppendageDataException(fmt::format("{} missing on {}", type_parameter.first, label)); + throw AppendageDataException(fmt::format("{} missing on {}", type_parameter.key(), label)); } - nlohmann::json parameter = m_data[type_parameter.first]; + nlohmann::json parameter = m_data[type_parameter.key()]; - if(type_parameter.second == "int") + if (type_parameter.key() == "core") + { + if (!type_parameter.value().is_array()) + { + throw AppendageDataException(fmt::format("Core field is not an array for type {}", type)); + } + + nlohmann::json& core_keys = type_parameter.value(); + for (nlohmann::json::iterator core_key = core_keys.begin(); core_key != core_keys.end(); ++core_key) + { + if (m_data.find(core_key.value().get()) == m_data.end()) + { + throw AppendageDataException(fmt::format("{} missing core field {}", label, core_key.value().get())); + } + + m_core_fields.emplace_back(core_key.value().get()); + } + } + else if(type_parameter.value() == "int") { if(!parameter.is_number_integer()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be an integer.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "float") + else if(type_parameter.value() == "float") { if(!parameter.is_number_float()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be an integer.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "string") + else if(type_parameter.value() == "string") { if(!parameter.is_string()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be a string.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "bool") + else if(type_parameter.value() == "bool") { if(!parameter.is_boolean()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be a bool.", - type_parameter.first, label)); + type_parameter.key(), label)); } } - else if(type_parameter.second == "object") + else if(type_parameter.value() == "object") { if(!parameter.is_object()) { throw AppendageDataException(fmt::format("Incorrect type for {} on {}. Should be an object.", - type_parameter.first, label)); + type_parameter.key(), label)); } } else { - throw AppendageDataException(fmt::format("Unknown Type in template file for {}", type_parameter.first, + throw AppendageDataException(fmt::format("Unknown Type in template file for {}", type_parameter.key(), label)); } } From a1cf6716ec7ee230688ecc916c58ab50e1ffa172 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 27 Mar 2018 21:01:52 -0400 Subject: [PATCH 081/200] Arduino code is now crashing... --- arduino_gen/code_template.txt | 2 +- .../include/cmd_messenger/cmd_messenger.hpp | 62 +++++++++++++++++-- .../include/cmd_messenger/device.hpp | 2 +- .../include/cmd_messenger/exceptions.hpp | 1 + 4 files changed, 60 insertions(+), 7 deletions(-) diff --git a/arduino_gen/code_template.txt b/arduino_gen/code_template.txt index 2ccebfb..5d810af 100644 --- a/arduino_gen/code_template.txt +++ b/arduino_gen/code_template.txt @@ -57,7 +57,7 @@ void attachCommandCallbacks() // Called when a received command has no attached function void unknownCommand() {{ - cmdMessenger.sendCmd(kError, kUnknown); + cmdMessenger.sendBinCmd(kError, kUnknown); }} // Called upon initialization of Spine to check connection diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index 07bc20d..17d2c77 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -137,6 +137,7 @@ namespace rip template void send(std::shared_ptr device, std::shared_ptr command, Args... args) { + std::string debugString; // HACK if (device == nullptr) { throw EmptyDevice(); @@ -147,6 +148,8 @@ namespace rip throw EmptyCommand(); } + std::cout << fmt::format("Command->getEnum(): {} (aka {})", command->getEnum(), command->getId()) << std::endl; + // Get the argument types std::string argument_types = command->getArgumentTypes(); const std::size_t value = sizeof...(Args); @@ -155,6 +158,10 @@ namespace rip throw IncorrectArgumentListSize(); } + debugString = byteStringToHexDebugString(toBytes(command->getEnum())); + std::cout << fmt::format("toBytes(command->getEnum()) bytes: {}", debugString) << std::endl; + // std::cout << toBytes(command->getEnum()) << std::endl; + // Pack the command to send std::string message = toBytes(command->getEnum()) + static_cast(m_field_separator); @@ -171,11 +178,24 @@ namespace rip } // Send the message + debugString = byteStringToHexDebugString(message); + std::cout << fmt::format("Device->write bytes: {}", debugString) << std::endl; device->write(message); + // HACK + device->setTimeout(units::s * 3); + // Check Acknowledgement std::string acknowledgement = device->readline(m_max_response_length, std::to_string(m_command_separator)); + if (acknowledgement.length() == 0) + { + throw EmptyDeviceResponse(fmt::format("did not receive any bytes from the device, timeout or crash?")); + } + + debugString = byteStringToHexDebugString(acknowledgement); + std::cout << fmt::format("Device->readline bytes: {}", debugString) << std::endl; + handleAck(acknowledgement, command); m_last_device = device; @@ -193,17 +213,17 @@ namespace rip */ void handleAck(std::string& acknowledgement, std::shared_ptr command) { - //std::string debugString = byteStringToHexDebugString(acknowledgement); + std::string debugString = byteStringToHexDebugString(acknowledgement); + std::cout << fmt::format("Ack Str: {}", debugString) << std::endl; // First part should be the acknowledgment id - T_IntegerType acknowledgement_id = fromBytes(acknowledgement); + uint8_t acknowledgement_id = fromBytes(acknowledgement); if (acknowledgement_id != 0) //TODO(Andrew): Look up number { - //std::cout << fmt::format("Ack Str: {}\nAck Id: {}", debugString, acknowledgement_id) << std::endl; - throw IncorrectAcknowledgementCommand("Acknowledge command incorrect"); + throw IncorrectAcknowledgementCommand(fmt::format("handleAck: Acknowledge command incorrect, got {} instead of zero.", acknowledgement_id)); } - + acknowledgement.erase(0,2); // Then the field separator if (acknowledgement[0] != m_field_separator) { @@ -309,6 +329,19 @@ namespace rip return str; } + // HACK + using byte = unsigned char ; + + template< typename T > std::array< byte, sizeof(T) > to_bytes( const T& object ) + { + std::array< byte, sizeof(T) > bytes; + + const byte* begin = reinterpret_cast< const byte* >( std::addressof(object) ) ; + const byte* end = begin + sizeof(T) ; + std::copy( begin, end, std::begin(bytes) ) ; + + return bytes; + } /** * @brief Convert a type into bytes @@ -331,6 +364,7 @@ namespace rip template typename std::enable_if::value, std::string>::type toBytes(const From& f) { + /* To t = static_cast(f); if (std::is_same::value) @@ -353,6 +387,22 @@ namespace rip byte_pointer ++; } return rv; + //*/ + To t = static_cast(f); + + if (std::is_same::value) + { + return toBytesString(t); + } + + std::string rv; + for (auto a_chr : to_bytes(t)) + { + char n_chr = static_cast(a_chr); + rv += n_chr; + } + std::reverse(rv.begin(), rv.end()); + return rv; } /** @@ -454,6 +504,8 @@ namespace rip // Recurse through future elements message += tupleToBytes < I + 1, Args... > (argument_types, args_tuple); + // Remove comma and add semi-colon + return message; } diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/device.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/device.hpp index b222212..a784d93 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/device.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/device.hpp @@ -43,7 +43,7 @@ namespace rip * @param baud_rate The speed for sending and receiving messages * @param timeout The timeout to allow for communication */ - Device(std::string port, unsigned long baud_rate = 115200, units::Time timeout = 1.0); + Device(std::string port, unsigned long baud_rate = 115200, units::Time timeout = units::s * 1.0); /** * @brief Sets the timeout to allow for communication diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp index daced1b..cf62c7b 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp @@ -27,6 +27,7 @@ namespace rip { NEW_EX(EmptyDevice) NEW_EX(EmptyCommand) + NEW_EX(EmptyDeviceResponse) NEW_EX(IncorrectAcknowledgementCommand) NEW_EX(IncorrectArgumentListSize) From f44b66a6d70903787d21c4c77fdf409dca0a23fb Mon Sep 17 00:00:00 2001 From: Andrew Messing Date: Wed, 28 Mar 2018 13:27:17 -0400 Subject: [PATCH 082/200] set max response length --- .../cmd_messenger/include/cmd_messenger/cmd_messenger.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index 17d2c77..0617d44 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -117,7 +117,8 @@ namespace rip * @param escape_character The character used to escape the separators */ CmdMessenger(char field_separator = ',', char command_separator = ';', char escape_character = '/') - : m_field_separator(field_separator) + : m_max_response_length(1023) + , m_field_separator(field_separator) , m_command_separator(command_separator) , m_escape_character(escape_character) , m_last_device(nullptr) @@ -181,13 +182,14 @@ namespace rip debugString = byteStringToHexDebugString(message); std::cout << fmt::format("Device->write bytes: {}", debugString) << std::endl; device->write(message); - + // HACK device->setTimeout(units::s * 3); // Check Acknowledgement std::string acknowledgement = device->readline(m_max_response_length, std::to_string(m_command_separator)); + if (acknowledgement.length() == 0) { throw EmptyDeviceResponse(fmt::format("did not receive any bytes from the device, timeout or crash?")); From 0f2cd03179caabc995f448414fba7f9faf4cd580 Mon Sep 17 00:00:00 2001 From: Ben Johnson Date: Wed, 28 Mar 2018 13:29:36 -0400 Subject: [PATCH 083/200] more xml functions, fixed PID function names --- appendages/include/appendages/roboclaw.hpp | 3 +- appendages/xml/roboclaw.xml | 174 +++++++++++++++------ 2 files changed, 131 insertions(+), 46 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 1bfa419..e50a0b5 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -56,7 +56,8 @@ namespace rip void setDuties(int16_t duty1, int16_t duty2); //Not implemented on arduino void setDuty(bool motor, int16_t duty); //Not implemented on arduino - void setVelocityPID(bool motor, float Kp, float Ki, float Kd, uint32_t qpps); + void setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps); + void setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps); std::array getBuffers(); void resetEncoders(); diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 1c1a396..592ced1 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -27,7 +27,8 @@ - if (!roboclaws[indexNum].SpeedM2(address, speed)) { + if (!roboclaws[indexNum].SpeedM2(address, speed)) + { cmdMessenger.sendBinCmd(kerror, kSetM2Speed); } @@ -37,7 +38,8 @@ - if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) { + if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) + { cmdMessenger.sendBinCmd(kerror, kSetM1M2Speed); } @@ -49,7 +51,8 @@ - if (!roboclaws[indexNum].SpeedAccelM1(address, accel, speed)) { + if (!roboclaws[indexNum].SpeedAccelM1(address, accel, speed)) + { cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccel); } @@ -59,7 +62,8 @@ - if (!roboclaws[indexNum].SpeedAccelM2(address, accel, speed)) { + if (!roboclaws[indexNum].SpeedAccelM2(address, accel, speed)) + { cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccel); } @@ -70,7 +74,8 @@ - if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) { + if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) + { cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedAccel); } @@ -82,29 +87,32 @@ - if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance)) { + if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance)) + { cmdMessenger.sendBinCmd(kerror, kSetM1SpeedDist); } - + - if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance)) { + if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance)) + { cmdMessenger.sendBinCmd(kerror, kSetM2SpeedDist); } - + - + - if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2)) { + if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2)) + { cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedDist); } @@ -114,10 +122,11 @@ - + - if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance)) { + if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance)) + { cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccelDist); } @@ -125,10 +134,11 @@ - + - if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance)) { + if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance)) \ + { cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDist); } @@ -136,12 +146,13 @@ - + - + - if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2)) { + if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2)) + { cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedAccelDist); } @@ -151,11 +162,12 @@ - + - if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position)) { + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position)) + { cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccelDecelDist); } @@ -163,11 +175,12 @@ - + - if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position)) { + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position)) + { cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDecelDist); } @@ -175,15 +188,16 @@ - + - + - if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2)) { + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2)) + { cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDecelDist); } @@ -192,24 +206,35 @@ - + - + uint32_t tmp; + + if (!roboclaws[indexNum].ReadEncoders(address, enc1, tmp)) + { + cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); + } - + - + uint32_t tmp; + + if (!roboclaws[indexNum].ReadEncoders(address, tmp, enc2)) + { + cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); + } - - + + - if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) { + if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) + { cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); } @@ -218,62 +243,121 @@ - + + enc1Speed = roboclaws[indexNum].ReadSpeedM1(address, status, valid); + if (!valid) + { + cmdMessenger.sendBinCmd(kerror, kReadM1EncoderSpeed); + } + - + - bool RoboClaw::ReadEncoders(uint8_t address,uint32_t &enc1,uint32_t &enc2){ - + enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, status, valid); + if (!valid) + { + cmdMessenger.sendBinCmd(kerror, kReadM2EncoderSpeed); + } + - - + + + enc1Speed = roboclaws[indexNum].ReadSpeedM2(address, status, validM1); + enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, status, validM2); + + if (!validM1 || !validM2) + { + cmdMessenger.sendBinCmd(kerror, kReadM1M2EncoderSpeed); + } - + + if (!roboclaws[indexNum].DutyM1(address, duty)) + { + cmdMessenger.sendBinCmd(kerror, kSetM1Duty); + } - + + if (!roboclaws[indexNum].DutyM2(address, duty)) + { + cmdMessenger.sendBinCmd(kerror, kSetM2Duty); + } + + + + + + + + if (!roboclaws[indexNum].DutyM1M2(address, duty1, duty2)) + { + cmdMessenger.sendBinCmd(kerror, kSetM1M2Duty); + } - + + + + + + + + if (!roboclaws[indexNum].SetM1VelocityPID(address, kp, ki, kd, qpps)) + { + cmdMessenger.sendBinCmd(kerror, kSetM1VelocityPID); + } + + + - + if (!roboclaws[indexNum].SetM2VelocityPID(address, kp, ki, kd, qpps)) + { + cmdMessenger.sendBinCmd(kerror, kSetM2VelocityPID); + } - + - + + if (!roboclaws[indexNum].ReadBuffers(address, buff1, buff2)) + { + cmdMessenger.sendBinCmd(kerror, kGetBuffers); + } + if (!roboclaws[indexNum].ResetEncoders(address)) + { + cmdMessenger.sendBinCmd(kerror, kGetBuffers); + } - From ae223f958f201227ffcc9a6419cae1f024873c21 Mon Sep 17 00:00:00 2001 From: Aiden Date: Wed, 28 Mar 2018 13:37:24 -0400 Subject: [PATCH 084/200] progress towards Roboclaw appendage --- appendages/include/appendages/roboclaw.hpp | 30 ++-- appendages/src/roboclaw.cpp | 177 +++++++++++++++------ 2 files changed, 144 insertions(+), 63 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 1bfa419..4ef6ea4 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -20,7 +20,7 @@ namespace rip public: void setM1Speed(int32_t speed); void setM2Speed(int32_t speed); - void setM1M2Speed(int32_t speed); + void setM1M2Speed(int32_t speed1, int32_t speed2); void setM1SpeedAccel(uint32_t accel, int32_t speed); void setM2SpeedAccel(uint32_t accel, int32_t speed); @@ -41,27 +41,27 @@ namespace rip int32_t readM1Encoder(); int32_t readM2Encoder(); - std::array readM1M2Encoders(); + std::tuple readM1M2Encoders(); std::array readEncoders(); //Not implemented on arduino units::Distance readEncoder(bool motor); //Not implemented on arduino int32_t readM1EncoderSpeed(); int32_t readM2EncoderSpeed(); - std::array readM1M2EncoderSpeed(); + std::tuple readM1M2EncoderSpeed(); std::array readEncoderSpeeds(); //Not implemented on arduino units::Velocity readEncoderSpeed(bool motor); //Not implemented on arduino + void resetEncoders(); + std::tuple getBuffers(); + void setM1Duty(int16_t duty); void setM2Duty(int16_t duty); - void setDuties(int16_t duty1, int16_t duty2); //Not implemented on arduino - void setDuty(bool motor, int16_t duty); //Not implemented on arduino + void setM1M2Duty(int16_t duty1, int16_t duty2); void setVelocityPID(bool motor, float Kp, float Ki, float Kd, uint32_t qpps); - std::array getBuffers(); - void resetEncoders(); - void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=true); //Not implemented on arduino - void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=true); //Not implemented on arduino + void setDynamics(bool motor, const MotorDynamics& dynamics); //Not implemented on arduino + void setDynamics(const MotorDynamics& dynamics); //Not implemented on arduino /** * Stop! ^0^ */ @@ -121,6 +121,12 @@ namespace rip std::shared_ptr m_read_encoder_result; std::shared_ptr m_read_encoders_result; + std::shared_ptr m_read_m1_encoder_speed; + std::shared_ptr m_read_m2_encoder_speed; + std::shared_ptr m_read_m1m2_encoder_speed; + std::shared_ptr m_read_encoder_speed_result; + std::shared_ptr m_read_encoders_speed_result; + std::shared_ptr m_reset_encoders; std::shared_ptr m_get_buffers; std::shared_ptr m_get_buffers_result; @@ -131,12 +137,6 @@ namespace rip std::shared_ptr m_set_speed_pid; - std::shared_ptr m_read_m1_encoder_speed; - std::shared_ptr m_read_m2_encoder_speed; - std::shared_ptr m_read_m1m2_encoder_speed; - std::shared_ptr m_read_encoder_speed_result; - std::shared_ptr m_read_encoders_speed_result; - }; } } diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 5cfdde4..486ff27 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -85,17 +85,24 @@ namespace rip m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_m1_encoder_speed(createCommand("kReadM1Encoder", command_map, + m_read_m1_encoder_speed(createCommand("kReadM1EncoderSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_m2_encoder_speed(createCommand("kReadM2Encoder", command_map, + m_read_m2_encoder_speed(createCommand("kReadM2EncoderSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_m1m2_encoder_speed(createCommand("kReadM1M2Encoder", command_map, + m_read_m1m2_encoder_speed(createCommand("kReadM1M2EncoderSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoder_speed_result(createCommand("kReadEncoderSpeedResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoders_speed_result(createCommand("kReadEncodersSpeedResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_reset_encoders(createCommand("kResetEncoders", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_get_buffers(createCommand("kGetBuffers", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_get_buffers_result(createCommand("kReadEncodersSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_duty(createCommand("kSetM1Duty", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), @@ -139,17 +146,24 @@ namespace rip void Roboclaw::setM1Speed(int32_t speed) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed, m_address, speed); } void Roboclaw::setM2Speed(int32_t speed) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed, m_address, speed); } - void Roboclaw::setM1M2Speed(int32_t speed) + void Roboclaw::setM1M2Speed(int32_t speed1, speed2) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed, m_address, speed1, speed2); } void Roboclaw::setM1SpeedAccel(uint32_t accel, int32_t speed) @@ -164,6 +178,8 @@ namespace rip void Roboclaw::setM1M2SpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); } @@ -222,11 +238,12 @@ namespace rip } - std::array Roboclaw::readM1M2Encoders() + std::tuple Roboclaw::readM1M2Encoders() { } + units::Distance Roboclaw::readEncoder(bool motor) { cmdmessenger::ArduinoCmdMessenger messenger; @@ -242,7 +259,7 @@ namespace rip } - std::array Roboclaw::readM1M2EncoderSpeed() + std::tuple Roboclaw::readM1M2EncoderSpeed() { } @@ -252,37 +269,27 @@ namespace rip } - void Roboclaw::resetEncoders() + units::Velocity Roboclaw::readEncoderSpeed(bool motor) { } - units::Velocity Roboclaw::readEncoderSpeed(bool motor) + void Roboclaw::resetEncoders() { } - /* - void Roboclaw::setSpeed(int32_t speed1, int32_t speed2) - { - cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_speed, m_address, speed1, speed2); - } - void Roboclaw::setSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) + std::tuple getBuffers() { - cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + } std::array Roboclaw::readEncoders() { - cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_encoders, m_address); - return messenger.receive(m_read_encoders_result); + } - */ - void Roboclaw::setDynamic(bool motor, const MotorDynamics& dynamics, bool respectBuffer) + void Roboclaw::setDynamic(bool motor, const MotorDynamics& dynamics) { cmdmessenger::ArduinoCmdMessenger messenger; int32_t speed; @@ -297,25 +304,74 @@ namespace rip case MotorDynamics::DType::kSpeed: { speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); - + if(motor) + { + setM2Speed(speed); + } + else + { + setM1Speed(speed); + } + return; } case MotorDynamics::DType::kSpeedAccel: { speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); accel = static_cast((*dynamics.getAcceleration() / (m_wheel_radius * M_PI * 2)).to(1 / (units::s * units::s)) * m_ticks_per_rev); - + if(motor) + { + setM2SpeedAccel(accel, speed); + } + else + { + setM1SpeedAccel(accel, speed); + } + return; } case MotorDynamics::DType::kSpeedDist: { - + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / (m_wheel_radius * M_PI * 2)).to(units::none) * m_ticks_per_rev); + if(motor) + { + setM2SpeedDist(speed, dist); + } + else + { + setM1SpeedDist(speed, dist); + } + return; } case MotorDynamics::DType::kSpeedAccelDist: { - + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + if(motor) + { + setM2SpeedAccelDist(accel, speed, dist); + } + else + { + setM1SpeedAccelDist(accel, speed, dist); + } + return; } case MotorDynamics::DType::kSpeedAccelDecelDist: { - + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + if(motor) + { + setM2SpeedAccelDecelDist(accel, speed, decel, dist); + } + else + { + setM1SpeedAccelDecelDist(accel, speed, decel, dist); + } + return; } default: { @@ -324,7 +380,7 @@ namespace rip } } - void Roboclaw::setDynamics(const MotorDynamics& dynamics, bool respectBuffer); + void Roboclaw::setDynamics(const MotorDynamics& dynamics); { cmdmessenger::ArduinoCmdMessenger messenger; int32_t speed; @@ -334,31 +390,44 @@ namespace rip { case MotorDynamics::DType::kNone: { - + return; } case MotorDynamics::DType::kSpeed: { - + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + setM1M2Speed(speed, speed); + return; } case MotorDynamics::DType::kSpeedAccel: { - messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / (m_wheel_radius * M_PI * 2)).to(1 / (units::s * units::s)) * m_ticks_per_rev); + setM1M2SpeedAccel(accel, speed, speed); + return; } case MotorDynamics::DType::kSpeedDist: { - - } - case MotorDynamics::DType::kSpeedDist: - { - + speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / (m_wheel_radius * M_PI * 2)).to(units::none) * m_ticks_per_rev); + setM1M2SpeedDist(speed, dist, speed, dist); + return; } case MotorDynamics::DType::kSpeedAccelDist: { - + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + setM1M2SpeedAccelDist(accel, speed, dist, speed, dist); + return; } case MotorDynamics::DType::kSpeedAccelDecelDist: { - + speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); + setM1M2SpeedAccelDecelDist(accel, speed, decel, dist, accel, speed, decel, dist); + return; } default: { @@ -367,16 +436,28 @@ namespace rip } } - std::array Roboclaw::getBuffers(); + std::tuple Roboclaw::getBuffers(); + { + cmdmessenger::ArduinoCmdMessenger messenger; + } + + void Roboclaw::setM1Duty(int16_t duty) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_duty, m_address, duty); + } + + void Roboclaw::setM2Duty(int16_t duty) { cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_duty, m_address, duty); } - /* - void Roboclaw::setDuty(int16_t duty1, int16_t duty2) + + void Roboclaw::setM1M2Duty(int16_t duty1, int16_t duty2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_duty, m_address, duty1, duty2); - }*/ + messenger.send(m_device, m_set_m1m2_duty, m_address, duty1, duty2); + } void Roboclaw::setVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) { @@ -386,7 +467,7 @@ namespace rip void Roboclaw::stop() { - setDuty(0, 0); + setM1M2Duty(0, 0); } bool Roboclaw::diagnostic() @@ -398,7 +479,7 @@ namespace rip misc::Logger::getInstance()->debug("Read encoders for 10s in a loop"); while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) { - misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: ", std::get<0>(ReadEncoders()), std::get<0>(ReadEncoders()))); + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<0>(ReadEncoders()))); } misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); SetDuty(16000, 16000); From 567820d215d95ebca78d3af88decc082ea5badae Mon Sep 17 00:00:00 2001 From: Aiden Date: Wed, 28 Mar 2018 16:09:06 -0400 Subject: [PATCH 085/200] Updated roboclaw appendage compiles --- appendages/CMakeLists.txt | 2 +- appendages/include/appendages/roboclaw.hpp | 4 +- appendages/src/roboclaw.cpp | 193 ++++++++++++++++----- 3 files changed, 151 insertions(+), 48 deletions(-) diff --git a/appendages/CMakeLists.txt b/appendages/CMakeLists.txt index 0bcf991..2e7a756 100644 --- a/appendages/CMakeLists.txt +++ b/appendages/CMakeLists.txt @@ -9,7 +9,7 @@ add_library(${PROJECT_NAME} EXCLUDE_FROM_ALL ${${PROJECT_NAME}_SOURCES} ${${PROJ set_property(TARGET ${PROJECT_NAME} PROPERTY CXX_STANDARD 11) target_link_libraries(${PROJECT_NAME} fmt json spdlog cppfs - cmd_messenger units misc) + cmd_messenger units misc motor_controllers) target_include_directories(${PROJECT_NAME} PUBLIC include) set_property(GLOBAL PROPERTY AppendageRoot ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 8181241..144683e 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -15,6 +15,7 @@ namespace rip { namespace appendages { + using MotorDynamics = rip::motorcontrollers::MotorDynamics; class Roboclaw : public Appendage { public: @@ -136,7 +137,8 @@ namespace rip std::shared_ptr m_set_m2_duty; std::shared_ptr m_set_m1m2_duty; - std::shared_ptr m_set_speed_pid; + std::shared_ptr m_set_m1_velocity_pid; + std::shared_ptr m_set_m2_velocity_pid; }; } diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 486ff27..2c1cedc 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -113,13 +113,19 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, + m_set_m1_velocity_pid(createCommand("kSetVelocityPID", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_velocity_pid(createCommand("kSetVelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + { if (config.find("address") == config.end()) { @@ -155,141 +161,235 @@ namespace rip { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m2_speed, m_address, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed, m_address, speed); } - void Roboclaw::setM1M2Speed(int32_t speed1, speed2) + void Roboclaw::setM1M2Speed(int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1m2_speed, m_address, speed1, speed2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed, m_address, speed1, speed2); } void Roboclaw::setM1SpeedAccel(uint32_t accel, int32_t speed) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_accel, m_address, accel, speed); } - void Roboclaw::setM2SpeedAccel(int32_t accel, int32_t speed) + void Roboclaw::setM2SpeedAccel(uint32_t accel, int32_t speed) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_accel, m_address, accel, speed); } - void Roboclaw::setM1M2SpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) + void Roboclaw::setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); - + messenger.send(m_device, m_set_m1m2_speed_accel, m_address, accel, speed1, speed2); } void Roboclaw::setM1SpeedDist(int32_t speed, uint32_t distance) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_dist, m_address, speed, distance); } void Roboclaw::setM2SpeedDist(int32_t speed, uint32_t distance) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_dist, m_address, speed, distance); } void Roboclaw::setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed_dist, m_address, speed1, distance1, speed2, distance2); } void Roboclaw::setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_accel_dist, m_address, accel, speed, distance); } void Roboclaw::setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_accel_dist, m_address, accel, speed, distance); } void Roboclaw::setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1m2_speed_accel_dist, m_address, accel, speed1, distance1, speed2, distance2); } void Roboclaw::setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_speed_accel_decel_dist, m_address, accel, speed, deccel, position); } void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m2_speed_accel_decel_dist, m_address, accel, speed, deccel, position); } - void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) + void Roboclaw::setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, + uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2) { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send( + m_device, m_set_m1m2_speed_accel_decel_dist, m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2); } int32_t Roboclaw::readM1Encoder() { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1_encoder, m_address); + return std::get<0>(messenger.receive(m_read_encoder_result)); } int32_t Roboclaw::readM2Encoder() { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m2_encoder, m_address); + return std::get<0>(messenger.receive(m_read_encoder_result)); } std::tuple Roboclaw::readM1M2Encoders() { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1m2_encoder, m_address); + return messenger.receive(m_read_encoders_result); } units::Distance Roboclaw::readEncoder(bool motor) { - cmdmessenger::ArduinoCmdMessenger messenger; + if(motor) + { + return static_cast(readM2Encoder()) / m_ticks_per_rev * m_wheel_radius * M_PI * 2; + } + else + { + return static_cast(readM1Encoder()) / m_ticks_per_rev * m_wheel_radius * M_PI * 2; + } } - int32_t Roboclaw::readM1EncoderSpeed() + std::array Roboclaw::readEncoders() { + std::array d; + std::tuple ticks; + ticks = readM1M2Encoders(); + d[0] = static_cast(std::get<0>(ticks)) / m_ticks_per_rev * m_wheel_radius * M_PI * 2; + d[1] = static_cast(std::get<1>(ticks)) / m_ticks_per_rev * m_wheel_radius * M_PI * 2; + return d; } - int32_t Roboclaw::readM2EncoderSpeed() + int32_t Roboclaw::readM1EncoderSpeed() { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1_encoder_speed, m_address); + return std::get<0>(messenger.receive(m_read_encoder_speed_result)); + } + int32_t Roboclaw::readM2EncoderSpeed() + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m2_encoder_speed, m_address); + return std::get<0>(messenger.receive(m_read_encoder_speed_result)); } std::tuple Roboclaw::readM1M2EncoderSpeed() { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_read_m1m2_encoder_speed, m_address); + return messenger.receive(m_read_encoders_speed_result); } std::array Roboclaw::readEncoderSpeeds() { + std::array v; + std::tuple raw = readM1M2EncoderSpeed(); + + v[0] = static_cast(std::get<0>(raw)) / m_ticks_per_rev * m_wheel_radius * M_PI * 2 / units::s; + v[1] = static_cast(std::get<1>(raw)) / m_ticks_per_rev * m_wheel_radius * M_PI * 2 / units::s; + return v; } units::Velocity Roboclaw::readEncoderSpeed(bool motor) { - + units::Velocity v; + if(motor) + { + v = readM2EncoderSpeed() / m_ticks_per_rev * m_wheel_radius * M_PI * 2 / units::s; + } + else + { + v = readM1EncoderSpeed() / m_ticks_per_rev * m_wheel_radius * M_PI * 2 / units::s; + } + return v; } void Roboclaw::resetEncoders() { - - } - - std::tuple getBuffers() - { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_reset_encoders, m_address); } - std::array Roboclaw::readEncoders() + std::tuple Roboclaw::getBuffers() { - + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_get_buffers, m_address); + return messenger.receive(m_get_buffers_result); } - void Roboclaw::setDynamic(bool motor, const MotorDynamics& dynamics) + void Roboclaw::setDynamics(bool motor, const MotorDynamics& dynamics) { cmdmessenger::ArduinoCmdMessenger messenger; int32_t speed; @@ -380,7 +480,7 @@ namespace rip } } - void Roboclaw::setDynamics(const MotorDynamics& dynamics); + void Roboclaw::setDynamics(const MotorDynamics& dynamics) { cmdmessenger::ArduinoCmdMessenger messenger; int32_t speed; @@ -436,11 +536,6 @@ namespace rip } } - std::tuple Roboclaw::getBuffers(); - { - cmdmessenger::ArduinoCmdMessenger messenger; - } - void Roboclaw::setM1Duty(int16_t duty) { cmdmessenger::ArduinoCmdMessenger messenger; @@ -459,10 +554,16 @@ namespace rip messenger.send(m_device, m_set_m1m2_duty, m_address, duty1, duty2); } - void Roboclaw::setVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) + void Roboclaw::setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_set_m1_velocity_pid, m_address, Kp, Ki, Kd, qpps); + } + + void Roboclaw::setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_velocity_pid, m_address, motor, Kp, Ki, Kd, qpps); + messenger.send(m_device, m_set_m2_velocity_pid, m_address, Kp, Ki, Kd, qpps); } void Roboclaw::stop() From 1ef10bd13405342fa10ac46f5db7028b80a7bc2e Mon Sep 17 00:00:00 2001 From: Aiden Date: Wed, 28 Mar 2018 16:37:38 -0400 Subject: [PATCH 086/200] Missed a spot. --- appendages/include/appendages/roboclaw.hpp | 2 +- appendages/src/roboclaw.cpp | 22 +++++++++++++++++++--- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 144683e..e5ede7c 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -35,7 +35,7 @@ namespace rip void setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance); void setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2); - void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); + void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, uint32_t position); void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); void setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2); diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 2c1cedc..c38010f 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -68,12 +68,28 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>())), - m_set_m1m2_speed_accel_decel_dist(createCommand("kSetM1M2SpeedAccelDist", command_map, + m_set_m1_speed_accel_decel_dist(createCommand("kSetM1SpeedAccelDecelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m2_speed_accel_decel_dist(createCommand("kSetM2SpeedAccelDecelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_m1m2_speed_accel_decel_dist(createCommand("kSetM1M2SpeedAccelDecelDist", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m1_encoder(createCommand("kReadM1Encoder", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m2_encoder(createCommand("kReadM2Encoder", command_map, @@ -113,13 +129,13 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_m1_velocity_pid(createCommand("kSetVelocityPID", command_map, + m_set_m1_velocity_pid(createCommand("kSetM2VelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_m2_velocity_pid(createCommand("kSetVelocityPID", command_map, + m_set_m2_velocity_pid(createCommand("kSetM2VelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString Date: Wed, 28 Mar 2018 17:35:05 -0400 Subject: [PATCH 087/200] Fix arduinogen tests --- arduino_gen/test/test_arduino_gen.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/arduino_gen/test/test_arduino_gen.cpp b/arduino_gen/test/test_arduino_gen.cpp index 62523c2..be13f6a 100644 --- a/arduino_gen/test/test_arduino_gen.cpp +++ b/arduino_gen/test/test_arduino_gen.cpp @@ -594,7 +594,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -689,7 +689,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -788,7 +788,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -902,7 +902,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -1024,7 +1024,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -1395,7 +1395,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" @@ -1705,7 +1705,7 @@ namespace rip "// Called when a received command has no attached function\n" "void unknownCommand()\n" "{\n" - "\tcmdMessenger.sendCmd(kError, kUnknown);\n" + "\tcmdMessenger.sendBinCmd(kError, kUnknown);\n" "}\n" "\n" "// Called upon initialization of Spine to check connection\n" From fbddfdabdbd33c6d2a76401b4aeb5d3a93cbf3a2 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Wed, 28 Mar 2018 21:52:37 -0400 Subject: [PATCH 088/200] Update docker things with stretch support --- docker/core.sh | 3 ++- docker/make_chroot_tar_raspbian_jesse.sh | 3 +++ docker/rip_rpi.dockerfile | 21 +++++++++++++----- go-docker.sh | 28 ++++++++++++++++++++++-- 4 files changed, 47 insertions(+), 8 deletions(-) diff --git a/docker/core.sh b/docker/core.sh index 07779c7..4a1a241 100644 --- a/docker/core.sh +++ b/docker/core.sh @@ -12,7 +12,8 @@ else fi RPXC_SCRIPT_LOCATION=${RPXC_SCRIPT_LOCATION:-'./docker/rpi/rpxc/rpxc'} -RPXC_IMAGE=${RPXC_IMAGE:-'utkrobotics/rip_rpi:latest'} +RPXC_IMAGE_TAG=${RPXC_IMAGE_TAG:-'latest'} +RPXC_IMAGE=${RPXC_IMAGE:-"utkrobotics/rip_rpi:${RPXC_IMAGE_TAG}"} RPXC_SYSROOT=${RPXC_SYSROOT:-'/rpxc/sysroot'} function git_branch_norm() { diff --git a/docker/make_chroot_tar_raspbian_jesse.sh b/docker/make_chroot_tar_raspbian_jesse.sh index 589bc7d..8bff782 100644 --- a/docker/make_chroot_tar_raspbian_jesse.sh +++ b/docker/make_chroot_tar_raspbian_jesse.sh @@ -124,3 +124,6 @@ EOF ) #( set -x; rm -rf "$rootfsDir" ) + +echo "REMINDER: update the etc/apt/sources.list if you want additional repo selections other than 'main'!" + diff --git a/docker/rip_rpi.dockerfile b/docker/rip_rpi.dockerfile index 8d40b30..c6c2ca0 100644 --- a/docker/rip_rpi.dockerfile +++ b/docker/rip_rpi.dockerfile @@ -33,11 +33,22 @@ RUN cat /tmp/raspi-img.tar.xz \ RUN curl -fsSL https://github.com/resin-io-projects/armv7hf-debian-qemu/raw/master/bin/qemu-arm-static \ > $SYSROOT/$QEMU_PATH RUN chmod +x $SYSROOT/$QEMU_PATH + +ARG pi_image_dist +ENV PI_IMAGE_DIST=${pi_image_dist} +# edit the sources.list for more +RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c "\ + echo \"deb http://archive.raspbian.org/raspbian/ ${PI_IMAGE_DIST} main contrib non-free rpi\" > /etc/apt/sources.list " + +RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ + cd /dev \ + && MAKEDEV -n std \ + && MAKEDEV -d std \ + && MAKEDEV std \ + ' + RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ - echo "deb http://mirrordirector.raspbian.org/raspbian/ jessie main contrib non-free rpi" > /etc/apt/sources.list \ - && echo "deb http://archive.raspberrypi.org/debian/ jessie main ui" >> /etc/apt/sources.list \ - && apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 82B129927FA3303E \ - && apt-get clean \ + apt-get clean \ && apt-get update \ && DEBIAN_FRONTEND=noninteractive apt-get install -y apt-utils \ && DEBIAN_FRONTEND=noninteractive dpkg --configure -a \ @@ -48,7 +59,7 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ libc6-dev symlinks libssl-dev libcrypto++-dev \ libeigen3-dev libsuitesparse-dev qt5-default \ bash zsh git vim tmux \ - cmake lcov g++ time libssh-dev unzip \ + cmake lcov g++ time unzip \ && symlinks -cors / \ && apt-get clean' diff --git a/go-docker.sh b/go-docker.sh index d690a71..f8e8a45 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -61,11 +61,35 @@ function build_rip_deps() { function build_rip_rpi() { if ($PROMPTER --title "Comfirm Build?" --yesno "Do not run this command unless you were told to do so. Requires files outside of RIP." 8 68) then if find ../rpi_images -prune -empty ; then - rpi_image_file="$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | tail -1 )" + rpi_image_file="$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | sort -g | tail -1 )" echo "Found rpi debootstrap image ${rpi_image_file}" rsync --copy-links --times ../rpi_images/${rpi_image_file} ./external/rpi_images/ + if [ -z "$RPI_DOCKER_TAG" ]; then + RPI_DOCKER_TAG=$($PROMPTER --title "Docker tag" \ + --inputbox "Choose docker tag for building image ${rpi_image_file}" \ + $(( LINES - 4 )) $(( COLUMNS - 18 )) \ + "latest" \ + 3>&1 1>&2 2>&3 + ) + if [[ "$?" != "0" ]]; then + echo "Cancelled." + exit 0 + fi + fi + if [ -z "$RPI_DIST" ]; then + RPI_DIST=$($PROMPTER --title "Distro release" \ + --inputbox "Enter distro name of image ${rpi_image_file}" \ + $(( LINES - 4 )) $(( COLUMNS - 18 )) \ + "stretch" \ + 3>&1 1>&2 2>&3 + ) + if [[ "$?" != "0" ]]; then + echo "Cancelled." + exit 0 + fi + fi echo "Building the rip_rpi container... this will take awhile." - docker build -f docker/rip_rpi.dockerfile -t utkrobotics/rip_rpi:latest --build-arg pi_image=${rpi_image_file} . + docker build -f docker/rip_rpi.dockerfile -t utkrobotics/rip_rpi:${RPI_DOCKER_TAG} --build-arg pi_image=${rpi_image_file} --build-arg pi_image_dist=${RPI_DIST} . else echo "No Raspbian debootstrap images could be located at ../rpi_images." exit 1 From 04d653cb9853949796dcf2bc967ea297ed86c397 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Wed, 28 Mar 2018 23:04:52 -0400 Subject: [PATCH 089/200] Ascii commandID headers support --- core/framework/include/framework/spine.hpp | 2 +- core/framework/src/spine.cpp | 24 +++++++-- .../include/cmd_messenger/cmd_messenger.hpp | 53 ++++++++++++++----- .../include/cmd_messenger/exceptions.hpp | 6 +-- 4 files changed, 63 insertions(+), 22 deletions(-) diff --git a/core/framework/include/framework/spine.hpp b/core/framework/include/framework/spine.hpp index 91a83cf..7080820 100644 --- a/core/framework/include/framework/spine.hpp +++ b/core/framework/include/framework/spine.hpp @@ -116,7 +116,7 @@ namespace rip * @exception FileNotFound Thrown if the config file cannot be found * @exception IncorrectConfig Thrown if the config has incorrect information in it */ - void loadConfig(std::shared_ptr device, const std::string& path); + void loadConfig(std::shared_ptr device, const std::string& path, const std::string& device_name); /** * @brief Checks if possible to load a device diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index 724e33d..5e8747b 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -5,6 +5,7 @@ #include #include +#include #include namespace rip @@ -50,7 +51,7 @@ namespace rip { misc::Logger::getInstance()->debug(fmt::format("Loading device {}...", device_name)); m_devices[device_name] = std::make_shared(fmt::format("/dev/{}", device_name)); - loadConfig(m_devices[device_name], fmt::format("{}/{}/{}_core.json", arduino_gen_folder, device_name, device_name)); + loadConfig(m_devices[device_name], fmt::format("{0}/{1}/{1}_core.json", arduino_gen_folder, device_name), device_name); } } @@ -82,13 +83,13 @@ namespace rip return dev.exists() && config.exists(); } - void Spine::loadConfig(std::shared_ptr device, const std::string& path) + void Spine::loadConfig(std::shared_ptr device, const std::string& path, const std::string& device_name) { - misc::Logger::getInstance()->debug(fmt::format("Loading appendage config {} ...", path)); + misc::Logger::getInstance()->debug(fmt::format("Loading device config {} ...", path)); cppfs::FileHandle config_file = cppfs::fs::open(path); if (!config_file.exists()) { - misc::Logger::getInstance()->error(fmt::format("Cannot find appendage config file {}", path)); + misc::Logger::getInstance()->error(fmt::format("Cannot find device config file {}", path)); throw FileNotFound(fmt::format("Cannot find {}", path)); } @@ -105,6 +106,21 @@ namespace rip command_map[iter.key()] = iter.value(); } + // test the device by sending a kPing command + cmdmessenger::ArduinoCmdMessenger messenger; + std::shared_ptr command_ping; + std::shared_ptr command_ping_result; + command_ping = std::make_shared("kPing", command_map.find("kPing")->second, ""); + command_ping_result = std::make_shared( + "kPingResult", + command_map.find("kPingResult")->second, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString() + ); + //appendages::Appendage::createCommand("kPing", command_map, ""); + misc::Logger::getInstance()->debug(fmt::format("Attempting to Ping {} ", device_name)); + messenger.send(device, command_ping); + messenger.receive(command_ping_result); + std::shared_ptr factory = appendages::AppendageFactory::getInstance(); nlohmann::json appendages_conf = config["appendages"]; for (nlohmann::json appendage : appendages_conf) diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index 0617d44..2dbe577 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -32,7 +32,7 @@ #include -#include "exceptions.hpp" +#include #include "command.hpp" #include "device.hpp" @@ -46,7 +46,7 @@ namespace rip * @brief Packs a series of arguments in to a command message that can be sent * * @tparam T_StringType The type for representing a string (may not work with any other than the default type) - * @tparam T_IntegerType The type for representing an integer + * @tparam T_IntegerType The type for representing a cross-platform integer * @tparam T_UnsignedIntegerType * @tparam T_LongType * @tparam T_UnsignedLongType @@ -159,12 +159,12 @@ namespace rip throw IncorrectArgumentListSize(); } - debugString = byteStringToHexDebugString(toBytes(command->getEnum())); - std::cout << fmt::format("toBytes(command->getEnum()) bytes: {}", debugString) << std::endl; + debugString = byteStringToHexDebugString(std::to_string(command->getEnum())); + std::cout << fmt::format("to_str(command->getEnum()) bytes: {}", debugString) << std::endl; // std::cout << toBytes(command->getEnum()) << std::endl; // Pack the command to send - std::string message = toBytes(command->getEnum()) + static_cast(m_field_separator); + std::string message = std::to_string(command->getEnum()) + static_cast(m_field_separator); std::tuple args_tuple(args...); @@ -182,7 +182,7 @@ namespace rip debugString = byteStringToHexDebugString(message); std::cout << fmt::format("Device->write bytes: {}", debugString) << std::endl; device->write(message); - + // HACK device->setTimeout(units::s * 3); @@ -219,30 +219,45 @@ namespace rip std::cout << fmt::format("Ack Str: {}", debugString) << std::endl; // First part should be the acknowledgment id - uint8_t acknowledgement_id = fromBytes(acknowledgement); + // (a two-byte integer by default) + // note that fromBytes consumes and modifies the target + // T_CharType acknowledgement_id = fromBytes(acknowledgement); + T_IntegerType acknowledgement_id = std::stoi(acknowledgement.substr(0, acknowledgement.find(m_field_separator)), nullptr); + acknowledgement.erase(0, acknowledgement.find(m_field_separator)); - if (acknowledgement_id != 0) //TODO(Andrew): Look up number + if (acknowledgement_id != 0) // kAcknowledge will always be zero { throw IncorrectAcknowledgementCommand(fmt::format("handleAck: Acknowledge command incorrect, got {} instead of zero.", acknowledgement_id)); } - acknowledgement.erase(0,2); + // XXX + debugString = byteStringToHexDebugString(acknowledgement); + std::cout << fmt::format("After checking ack_id: {}", debugString) << std::endl; // Then the field separator if (acknowledgement[0] != m_field_separator) { - throw IncorrectFieldSeparator(); + std::cout << fmt::format("Bad field_separator: Wanted '{}' but got '{}' !", m_field_separator, acknowledgement[0]) << std::endl; + throw IncorrectFieldSeparator(fmt::format("Wanted '{}' but got '{}' !", m_field_separator, acknowledgement[0])); } acknowledgement.erase(0, 1); + debugString = byteStringToHexDebugString(acknowledgement); + std::cout << fmt::format("After checking field_separator: {}", debugString) << std::endl; + // Then the command sent T_IntegerType acknowledge_command = fromBytes(acknowledgement); - if (acknowledge_command != command->getEnum()) + // NOTE fromBytes consumes the data from the string! + if ( acknowledge_command != static_cast(command->getEnum()) ) { + std::cout << fmt::format("Acknowledgement command {:d} is not the same as the current command {:d}", acknowledge_command, command->getEnum()) << std::endl; throw IncorrectAcknowledgementCommand(fmt::format("Acknowledgement command {} is not the same as the current command {}", acknowledge_command, command->getEnum())); } if (acknowledgement[0] != m_command_separator) { - throw IncorrectCommandSeparator(); + std::cout << fmt::format("Wanted '{}' but got '{}' !", m_command_separator, acknowledgement[0]) << std::endl; + throw IncorrectCommandSeparator(fmt::format("Wanted '{}' but got '{}' !", m_command_separator, acknowledgement[0])); } + + std::cout << fmt::format("Successfully acknowledged command {}.", command->getId()) << std::endl; } /** @@ -281,6 +296,11 @@ namespace rip // Unpack the message std::string response = m_last_device->readline(m_max_response_length, std::to_string(m_command_separator)); + if (response.size() == 0) + { + throw EmptyDeviceResponse("In CmdMessenger->receive(), device->receive() did not return anything. Timeout?"); + } + // Check that response command is correct T_IntegerType response_command_enum = fromBytes(response); @@ -572,17 +592,22 @@ namespace rip { T rv; char* byte_pointer = reinterpret_cast(&rv); - for (size_t i = 0; i < sizeof(rv); i++) + uint16_t escape_chars_skipped = 0; + for (size_t i = 0; i < (sizeof(rv) + escape_chars_skipped); i++) { // Skip the escape character if (message[i] == m_escape_character) { i++; + escape_chars_skipped++; } *byte_pointer = message[i]; byte_pointer ++; } - message.erase(0, sizeof(rv)); + message.erase(0, sizeof(rv)+escape_chars_skipped); + // reverse the return val + // char *Tstart = reinterpret_cast(&rv), *Tend = Tstart + sizeof(rv); + // std::reverse(Tstart, Tend); return rv; } diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp index cf62c7b..f62044d 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp @@ -17,8 +17,8 @@ * \$$ $$ | $$ | $$ \$$\ | $$ | $$| $$ \| $$ * \$$$$$$ \$$ \$$ \$$ \$$ \$$ \$$$$$$ \$$ */ -#ifndef EXCEPTIONS_HPP -#define EXCEPTIONS_HPP +#ifndef CMD_MESSENGER_EXCEPTIONS_HPP +#define CMD_MESSENGER_EXCEPTIONS_HPP #include namespace rip @@ -40,4 +40,4 @@ namespace rip NEW_EX(UnknownArgument) } } -#endif // EXCEPTIONS_HPP +#endif // CMD_MESSENGER_EXCEPTIONS_HPP From 854529392e2340ac5fbd787774f4e2f7179a91ea Mon Sep 17 00:00:00 2001 From: Anthony Stewart Date: Thu, 29 Mar 2018 03:12:11 -0400 Subject: [PATCH 090/200] Ping now pongs --- core/framework/src/spine.cpp | 3 ++- .../cmd_messenger/include/cmd_messenger/cmd_messenger.hpp | 8 ++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index 5e8747b..53b3b5f 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -119,7 +119,8 @@ namespace rip //appendages::Appendage::createCommand("kPing", command_map, ""); misc::Logger::getInstance()->debug(fmt::format("Attempting to Ping {} ", device_name)); messenger.send(device, command_ping); - messenger.receive(command_ping_result); + std::tuple ping_result = messenger.receive(command_ping_result); + misc::Logger::getInstance()->debug(fmt::format("Ping result is {}", std::get<0>(ping_result))); std::shared_ptr factory = appendages::AppendageFactory::getInstance(); nlohmann::json appendages_conf = config["appendages"]; diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index 2dbe577..98c588d 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -187,8 +187,7 @@ namespace rip device->setTimeout(units::s * 3); // Check Acknowledgement - std::string acknowledgement = device->readline(m_max_response_length, std::to_string(m_command_separator)); - + std::string acknowledgement = device->readline(m_max_response_length, std::string(1, m_command_separator)); if (acknowledgement.length() == 0) { @@ -294,7 +293,7 @@ namespace rip } // Unpack the message - std::string response = m_last_device->readline(m_max_response_length, std::to_string(m_command_separator)); + std::string response = m_last_device->readline(m_max_response_length, std::string(1, m_command_separator)); if (response.size() == 0) { @@ -302,7 +301,8 @@ namespace rip } // Check that response command is correct - T_IntegerType response_command_enum = fromBytes(response); + T_IntegerType response_command_enum = std::stoi(response.substr(0, response.find(m_field_separator)), nullptr); + response.erase(0, response.find(m_field_separator)); if (response_command_enum != command->getEnum()) { From f6bc66adf7ccd13029a7b829ced36017a8dd671a Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Thu, 29 Mar 2018 11:40:21 -0400 Subject: [PATCH 091/200] AG: perms, CM: kError case --- arduino_gen/src/arduino_gen.cpp | 2 +- arduino_gen/src/main.cpp | 2 ++ .../include/cmd_messenger/cmd_messenger.hpp | 11 +++++++++-- .../include/cmd_messenger/exceptions.hpp | 1 + 4 files changed, 13 insertions(+), 3 deletions(-) diff --git a/arduino_gen/src/arduino_gen.cpp b/arduino_gen/src/arduino_gen.cpp index 5305f0a..5cf28b5 100644 --- a/arduino_gen/src/arduino_gen.cpp +++ b/arduino_gen/src/arduino_gen.cpp @@ -169,7 +169,7 @@ namespace rip FileHandle platformio_ini = fs::open(fmt::format("{}/{}/platformio.ini", m_parent_folder, m_arduino)); if (!platformio_ini.writeFile(platformio_str)) { - throw FileIoException(fmt::format("Could not create platform.ini: \"{}\"", platformio_ini.path())); + throw FileIoException(fmt::format("Could not create platformio.ini: \"{}\"", platformio_ini.path())); } platformio_ini.setPermissions(FilePermissions::UserRead | FilePermissions::UserWrite | FilePermissions::GroupRead | FilePermissions::GroupWrite | diff --git a/arduino_gen/src/main.cpp b/arduino_gen/src/main.cpp index 13f2874..27806f5 100644 --- a/arduino_gen/src/main.cpp +++ b/arduino_gen/src/main.cpp @@ -143,5 +143,7 @@ int main(int argc, char* argv[]) } } + std::system(fmt::format("chmod -R g+w {}/{}", args::get(parent_folder), args::get(arduino)).c_str()); + return EXIT_SUCCESS; } diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index 98c588d..70903c2 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -224,10 +224,17 @@ namespace rip T_IntegerType acknowledgement_id = std::stoi(acknowledgement.substr(0, acknowledgement.find(m_field_separator)), nullptr); acknowledgement.erase(0, acknowledgement.find(m_field_separator)); - if (acknowledgement_id != 0) // kAcknowledge will always be zero + // kAcknowledge will always be zero + if (acknowledgement_id == 1) { - throw IncorrectAcknowledgementCommand(fmt::format("handleAck: Acknowledge command incorrect, got {} instead of zero.", acknowledgement_id)); + debugString = byteStringToHexDebugString(acknowledgement); + throw DeviceSentErrorResponse(fmt::format("handleAck: device returned 1:kError: {}", debugString)); } + else if (acknowledgement_id != 0) + { + throw IncorrectAcknowledgementCommand(fmt::format("handleAck: Acknowledge command incorrect, got {} instead of zero.", acknowledgement_id)); + } + // XXX debugString = byteStringToHexDebugString(acknowledgement); std::cout << fmt::format("After checking ack_id: {}", debugString) << std::endl; diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp index f62044d..3c98006 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp @@ -38,6 +38,7 @@ namespace rip NEW_EX(NoLastDeviceException) NEW_EX(UnconvertibleArgument) NEW_EX(UnknownArgument) + NEW_EX(DeviceSentErrorResponse) } } #endif // CMD_MESSENGER_EXCEPTIONS_HPP From e7c18a26b2584d610876bf84c89f84874a81a6f5 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Thu, 29 Mar 2018 12:21:36 -0400 Subject: [PATCH 092/200] Logger reasons --- core/utilities/misc/include/misc/logger.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/core/utilities/misc/include/misc/logger.hpp b/core/utilities/misc/include/misc/logger.hpp index 04f4898..a8a6a9b 100644 --- a/core/utilities/misc/include/misc/logger.hpp +++ b/core/utilities/misc/include/misc/logger.hpp @@ -43,7 +43,6 @@ namespace rip { std::vector sinks; sinks.push_back(std::make_shared()); - time_t now = time(0); cppfs::FileHandle logs_folder = cppfs::fs::open("logs"); if(!logs_folder.exists()) @@ -51,7 +50,14 @@ namespace rip logs_folder.createDirectory(); } - sinks.push_back(std::make_shared(fmt::format("logs/{}.txt", asctime(localtime(&now))))); + time_t now; + time(&now); + char curtime_iso[sizeof "2011-10-08T07:07:09Z"]; + strftime(curtime_iso, sizeof curtime_iso, "%FT%TZ", gmtime(&now)); + // this will work too, if your compiler doesn't support %F or %T: + //strftime(buf, sizeof buf, "%Y-%m-%dT%H:%M:%SZ", gmtime(&now)); + + sinks.push_back(std::make_shared(fmt::format("logs/{}.txt", curtime_iso))); std::shared_ptr logger = std::make_shared(constants::kLoggerName, begin(sinks), end(sinks)); spdlog::set_pattern("[%H:%M:%S %z] [thread %t] [%I] %v"); From 1f607c6baa9b2f3d954b4a4c5e0fb5c207cd39da Mon Sep 17 00:00:00 2001 From: Aiden Date: Thu, 29 Mar 2018 18:05:04 -0400 Subject: [PATCH 093/200] m_id fix, kerror->kError --- appendages/src/roboclaw.cpp | 224 +++++++++++++++++++++++------------- appendages/xml/roboclaw.xml | 54 ++++----- 2 files changed, 173 insertions(+), 105 deletions(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 0b116b9..b199e15 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -16,72 +16,87 @@ namespace rip Roboclaw::Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device), m_set_m1_speed(createCommand("kSetM1Speed", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_speed(createCommand("kSetM2Speed", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1m2_speed(createCommand("kSetM1M2Speed", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_speed_accel(createCommand("kSetM1SpeedAccel", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_speed_accel(createCommand("kSetM2SpeedAccel", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1m2_speed_accel(createCommand("kSetM1M2SpeedAccel", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_speed_dist(createCommand("kSetM1SpeedDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_speed_dist(createCommand("kSetM2SpeedDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1m2_speed_dist(createCommand("kSetM1M2SpeedDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_speed_accel_dist(createCommand("kSetM1SpeedAccelDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_speed_accel_dist(createCommand("kSetM2SpeedAccelDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1m2_speed_accel_dist(createCommand("kSetM1M2SpeedAccelDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_speed_accel_decel_dist(createCommand("kSetM1SpeedAccelDecelDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_speed_accel_decel_dist(createCommand("kSetM2SpeedAccelDecelDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1m2_speed_accel_decel_dist(createCommand("kSetM1M2SpeedAccelDecelDist", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m1_encoder(createCommand("kReadM1Encoder", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m2_encoder(createCommand("kReadM2Encoder", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m1m2_encoder(createCommand("kReadM1M2Encoder", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoder_result(createCommand("kReadEncoderResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m1_encoder_speed(createCommand("kReadM1EncoderSpeed", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m2_encoder_speed(createCommand("kReadM2EncoderSpeed", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m1m2_encoder_speed(createCommand("kReadM1M2EncoderSpeed", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoder_speed_result(createCommand("kReadEncoderSpeedResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_encoders_speed_result(createCommand("kReadEncodersSpeedResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_reset_encoders(createCommand("kResetEncoders", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_get_buffers(createCommand("kGetBuffers", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_get_buffers_result(createCommand("kReadEncodersSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_get_buffers_result(createCommand("kGetBuffersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_duty(createCommand("kSetM1Duty", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_duty(createCommand("kSetM2Duty", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1m2_duty(createCommand("kSetM1M2Duty", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m1_velocity_pid(createCommand("kSetM2VelocityPID", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_set_m2_velocity_pid(createCommand("kSetM2VelocityPID", command_map, - cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) - { if (config.find("address") == config.end()) { @@ -169,160 +196,177 @@ namespace rip void Roboclaw::setM1Speed(int32_t speed) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_speed, m_address, speed); + messenger.send(m_device, m_set_m1_speed, m_id, m_address, speed); } void Roboclaw::setM2Speed(int32_t speed) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_speed, m_address, speed); + messenger.send(m_device, m_set_m2_speed, m_id, m_address, speed); } void Roboclaw::setM1M2Speed(int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1m2_speed, m_address, speed1, speed2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed, m_id, m_address, speed1, speed2); } void Roboclaw::setM1SpeedAccel(uint32_t accel, int32_t speed) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_speed_accel, m_address, accel, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_speed_accel, m_id, m_address, accel, speed); } void Roboclaw::setM2SpeedAccel(uint32_t accel, int32_t speed) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_speed_accel, m_address, accel, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed_accel, m_id, m_address, accel, speed); } void Roboclaw::setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1m2_speed_accel, m_address, accel, speed1, speed2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed_accel, m_id, m_address, accel, speed1, speed2); } void Roboclaw::setM1SpeedDist(int32_t speed, uint32_t distance) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_speed_dist, m_address, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_speed_dist, m_id, m_address, speed, distance); } void Roboclaw::setM2SpeedDist(int32_t speed, uint32_t distance) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_speed_dist, m_address, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed_dist, m_id, m_address, speed, distance); } void Roboclaw::setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1m2_speed_dist, m_address, speed1, distance1, speed2, distance2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed_dist, m_id, m_address, speed1, distance1, speed2, distance2); } void Roboclaw::setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_speed_accel_dist, m_address, accel, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_speed_accel_dist, m_id, m_address, accel, speed, distance); } void Roboclaw::setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_speed_accel_dist, m_address, accel, speed, distance); - + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed_accel_dist, m_id, m_address, accel, speed, distance); } void Roboclaw::setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1m2_speed_accel_dist, m_address, accel, speed1, distance1, speed2, distance2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed_accel_dist, m_id, m_address, accel, speed1, distance1, speed2, distance2); } void Roboclaw::setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_speed_accel_decel_dist, m_address, accel, speed, deccel, position); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_speed_accel_decel_dist, m_id, m_address, accel, speed, deccel, position); } void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_speed_accel_decel_dist, m_address, accel, speed, deccel, position); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed_accel_decel_dist, m_id, m_address, accel, speed, deccel, position); } void Roboclaw::setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send( - m_device, m_set_m1m2_speed_accel_decel_dist, m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2); + m_device, m_set_m1m2_speed_accel_decel_dist, m_id, m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2); } int32_t Roboclaw::readM1Encoder() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_m1_encoder, m_address); + messenger.send(m_device, m_read_m1_encoder, m_id, m_address); return std::get<0>(messenger.receive(m_read_encoder_result)); } int32_t Roboclaw::readM2Encoder() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_m2_encoder, m_address); + messenger.send(m_device, m_read_m2_encoder, m_id, m_address); return std::get<0>(messenger.receive(m_read_encoder_result)); } std::tuple Roboclaw::readM1M2Encoders() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_m1m2_encoder, m_address); - return messenger.receive(m_read_encoders_result); + messenger.send(m_device, m_read_m1m2_encoder, m_id, m_address); + return messenger.receive(m_read_encoders_result); } - units::Distance Roboclaw::readEncoder(bool motor) { if(motor) @@ -349,22 +393,26 @@ namespace rip int32_t Roboclaw::readM1EncoderSpeed() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_m1_encoder_speed, m_address); + messenger.send(m_device, m_read_m1_encoder_speed, m_id, m_address); return std::get<0>(messenger.receive(m_read_encoder_speed_result)); } int32_t Roboclaw::readM2EncoderSpeed() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_m2_encoder_speed, m_address); + messenger.send(m_device, m_read_m2_encoder_speed, m_id, m_address); return std::get<0>(messenger.receive(m_read_encoder_speed_result)); } std::tuple Roboclaw::readM1M2EncoderSpeed() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_m1m2_encoder_speed, m_address); - return messenger.receive(m_read_encoders_speed_result); + messenger.send(m_device, m_read_m1m2_encoder_speed, m_id, m_address); + return messenger.receive(m_read_encoders_speed_result); } std::array Roboclaw::readEncoderSpeeds() @@ -395,14 +443,17 @@ namespace rip void Roboclaw::resetEncoders() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_reset_encoders, m_address); + messenger.send(m_device, m_reset_encoders, m_id, m_address); } std::tuple Roboclaw::getBuffers() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_get_buffers, m_address); - return messenger.receive(m_get_buffers_result); + messenger.send(m_device, m_get_buffers, m_id, m_address); + return messenger.receive(m_get_buffers_result); } void Roboclaw::setDynamics(bool motor, const MotorDynamics& dynamics) @@ -555,31 +606,48 @@ namespace rip void Roboclaw::setM1Duty(int16_t duty) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_duty, m_address, duty); + messenger.send(m_device, m_set_m1_duty, m_id, m_address, duty); } void Roboclaw::setM2Duty(int16_t duty) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_duty, m_address, duty); + messenger.send(m_device, m_set_m2_duty, m_id, m_address, duty); } void Roboclaw::setM1M2Duty(int16_t duty1, int16_t duty2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1m2_duty, m_address, duty1, duty2); + messenger.send(m_device, m_set_m1m2_duty, m_id, m_address, duty1, duty2); } void Roboclaw::setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m1_velocity_pid, m_address, Kp, Ki, Kd, qpps); + messenger.send(m_device, m_set_m1_velocity_pid, m_id, m_address, Kp, Ki, Kd, qpps); } void Roboclaw::setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_m2_velocity_pid, m_address, Kp, Ki, Kd, qpps); + messenger.send(m_device, m_set_m2_velocity_pid, m_id, m_address, Kp, Ki, Kd, qpps); } void Roboclaw::stop() diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 592ced1..7c38344 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -29,7 +29,7 @@ if (!roboclaws[indexNum].SpeedM2(address, speed)) { - cmdMessenger.sendBinCmd(kerror, kSetM2Speed); + cmdMessenger.sendBinCmd(kError, kSetM2Speed); } @@ -40,7 +40,7 @@ if (!roboclaws[indexNum].SpeedM1M2(address, speed1, speed2)) { - cmdMessenger.sendBinCmd(kerror, kSetM1M2Speed); + cmdMessenger.sendBinCmd(kError, kSetM1M2Speed); } @@ -53,7 +53,7 @@ if (!roboclaws[indexNum].SpeedAccelM1(address, accel, speed)) { - cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccel); + cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccel); } @@ -64,7 +64,7 @@ if (!roboclaws[indexNum].SpeedAccelM2(address, accel, speed)) { - cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccel); + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccel); } @@ -76,7 +76,7 @@ if (!roboclaws[indexNum].SpeedAccelM1M2(address, accel, speed1, speed2)) { - cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedAccel); + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccel); } @@ -89,7 +89,7 @@ if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance)) { - cmdMessenger.sendBinCmd(kerror, kSetM1SpeedDist); + cmdMessenger.sendBinCmd(kError, kSetM1SpeedDist); } @@ -100,7 +100,7 @@ if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance)) { - cmdMessenger.sendBinCmd(kerror, kSetM2SpeedDist); + cmdMessenger.sendBinCmd(kError, kSetM2SpeedDist); } @@ -113,7 +113,7 @@ if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2)) { - cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedDist); + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedDist); } @@ -127,7 +127,7 @@ if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance)) { - cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccelDist); + cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccelDist); } @@ -139,7 +139,7 @@ if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance)) \ { - cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDist); + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDist); } @@ -153,7 +153,7 @@ if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2)) { - cmdMessenger.sendBinCmd(kerror, kSetM1M2SpeedAccelDist); + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccelDist); } @@ -168,7 +168,7 @@ if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position)) { - cmdMessenger.sendBinCmd(kerror, kSetM1SpeedAccelDecelDist); + cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccelDecelDist); } @@ -181,7 +181,7 @@ if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position)) { - cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDecelDist); + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDecelDist); } @@ -198,7 +198,7 @@ if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2)) { - cmdMessenger.sendBinCmd(kerror, kSetM2SpeedAccelDecelDist); + cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDecelDist); } @@ -212,7 +212,7 @@ if (!roboclaws[indexNum].ReadEncoders(address, enc1, tmp)) { - cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); + cmdMessenger.sendBinCmd(kError, kReadM1M2Encoder); } @@ -224,7 +224,7 @@ if (!roboclaws[indexNum].ReadEncoders(address, tmp, enc2)) { - cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); + cmdMessenger.sendBinCmd(kError, kReadM1M2Encoder); } @@ -235,7 +235,7 @@ if (!roboclaws[indexNum].ReadEncoders(address, enc1, enc2)) { - cmdMessenger.sendBinCmd(kerror, kReadM1M2Encoder); + cmdMessenger.sendBinCmd(kError, kReadM1M2Encoder); } @@ -248,7 +248,7 @@ enc1Speed = roboclaws[indexNum].ReadSpeedM1(address, status, valid); if (!valid) { - cmdMessenger.sendBinCmd(kerror, kReadM1EncoderSpeed); + cmdMessenger.sendBinCmd(kError, kReadM1EncoderSpeed); } @@ -260,7 +260,7 @@ enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, status, valid); if (!valid) { - cmdMessenger.sendBinCmd(kerror, kReadM2EncoderSpeed); + cmdMessenger.sendBinCmd(kError, kReadM2EncoderSpeed); } @@ -274,7 +274,7 @@ if (!validM1 || !validM2) { - cmdMessenger.sendBinCmd(kerror, kReadM1M2EncoderSpeed); + cmdMessenger.sendBinCmd(kError, kReadM1M2EncoderSpeed); } @@ -286,7 +286,7 @@ if (!roboclaws[indexNum].DutyM1(address, duty)) { - cmdMessenger.sendBinCmd(kerror, kSetM1Duty); + cmdMessenger.sendBinCmd(kError, kSetM1Duty); } @@ -296,7 +296,7 @@ if (!roboclaws[indexNum].DutyM2(address, duty)) { - cmdMessenger.sendBinCmd(kerror, kSetM2Duty); + cmdMessenger.sendBinCmd(kError, kSetM2Duty); } @@ -307,7 +307,7 @@ if (!roboclaws[indexNum].DutyM1M2(address, duty1, duty2)) { - cmdMessenger.sendBinCmd(kerror, kSetM1M2Duty); + cmdMessenger.sendBinCmd(kError, kSetM1M2Duty); } @@ -322,7 +322,7 @@ if (!roboclaws[indexNum].SetM1VelocityPID(address, kp, ki, kd, qpps)) { - cmdMessenger.sendBinCmd(kerror, kSetM1VelocityPID); + cmdMessenger.sendBinCmd(kError, kSetM1VelocityPID); } @@ -335,7 +335,7 @@ if (!roboclaws[indexNum].SetM2VelocityPID(address, kp, ki, kd, qpps)) { - cmdMessenger.sendBinCmd(kerror, kSetM2VelocityPID); + cmdMessenger.sendBinCmd(kError, kSetM2VelocityPID); } @@ -346,7 +346,7 @@ if (!roboclaws[indexNum].ReadBuffers(address, buff1, buff2)) { - cmdMessenger.sendBinCmd(kerror, kGetBuffers); + cmdMessenger.sendBinCmd(kError, kGetBuffers); } @@ -355,7 +355,7 @@ if (!roboclaws[indexNum].ResetEncoders(address)) { - cmdMessenger.sendBinCmd(kerror, kGetBuffers); + cmdMessenger.sendBinCmd(kError, kGetBuffers); } From 9250ca7dd43f7d3ce2c591b391c848320dbed23f Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Thu, 29 Mar 2018 18:25:54 -0400 Subject: [PATCH 094/200] Fixed a lot of issues Arduino Comms now work! --- appendages/src/roboclaw.cpp | 90 ++++++++++-- appendages/src/servo.cpp | 21 ++- core/framework/src/spine.cpp | 6 +- .../include/cmd_messenger/cmd_messenger.hpp | 138 +++++++++++++----- .../include/cmd_messenger/exceptions.hpp | 2 + 5 files changed, 208 insertions(+), 49 deletions(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 4d0c79a..4af880a 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -15,11 +15,61 @@ namespace rip { Roboclaw::Roboclaw(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device), - m_set_speed(createCommand("kSetSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_speed_accel(createCommand("kSetSpeedAccel", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoders(createCommand("kReadEncoders", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoders_result(createCommand("kReadEncodersResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_set_duty(createCommand("kSetDuty", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_set_speed( + createCommand( + "kSetSpeed", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType + >() + ) + ), + m_set_speed_accel( + createCommand( + "kSetSpeedAccel", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType + >() + ) + ), + m_read_encoders( + createCommand( + "kReadEncoders", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType + >() + ) + ), + m_read_encoders_result(createCommand( + "kReadEncodersResult", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType + >() + ) + ), + m_set_duty( + createCommand( + "kSetDuty", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType, + cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>() + ) + ), m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) { if (config.find("address") == config.end()) @@ -37,26 +87,45 @@ namespace rip void Roboclaw::SetSpeed(int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_speed, m_address, speed1, speed2); + messenger.send< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType + >(m_device, m_set_speed, m_id, m_address, speed1, speed2); } void Roboclaw::SetSpeedAccel(int32_t accel, int32_t speed1, int32_t speed2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_speed_accel, m_address, accel, speed1, speed2); + messenger.send< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType + >(m_device, m_set_speed_accel, m_id, m_address, accel, speed1, speed2); } std::tuple Roboclaw::ReadEncoders() { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_read_encoders, m_address); + messenger.send< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::CharType + >( + m_device, + m_read_encoders, + m_id, + m_address + ); return messenger.receive(m_read_encoders_result); } void Roboclaw::SetDuty(int16_t duty1, int16_t duty2) { cmdmessenger::ArduinoCmdMessenger messenger; - messenger.send(m_device, m_set_duty, m_address, duty1, duty2); + messenger.send(m_device, m_set_duty, m_id, m_address, duty1, duty2); } void Roboclaw::SetVelocityPID(uint8_t motor, float Kp, float Ki, float Kd, uint32_t qpps) @@ -66,7 +135,7 @@ namespace rip } void Roboclaw::stop() { - SetSpeed(0, 0); + SetDuty(0, 0); } bool Roboclaw::diagnostic() @@ -87,6 +156,7 @@ namespace rip stop(); misc::Logger::getInstance()->debug("Setting speed accel drive (5s)"); SetSpeedAccel(12000, 12000, 12000); + start_time = std::chrono::system_clock::now(); while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) {} stop(); diff --git a/appendages/src/servo.cpp b/appendages/src/servo.cpp index e20d711..b1a0676 100644 --- a/appendages/src/servo.cpp +++ b/appendages/src/servo.cpp @@ -12,7 +12,15 @@ namespace rip { Servo::Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) : Appendage(config, device) - , m_write(createCommand("kServoWrite", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + , m_write( + createCommand( + "kSetServo", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::IntegerType>() + ) + ) { } @@ -29,7 +37,16 @@ namespace rip bool Servo::diagnostic() { - return true; + int new_val = 0; + while (new_val != -1) { + // write(0); + std::cout << " >>> Please enter a servo value (int) to write (-1 quits): "; + std::cin >> new_val; + if (new_val == -1) break; + std::cout << " >>> Writing...\n"; + write(new_val); + } + return true; } std::shared_ptr Servo::create(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device) diff --git a/core/framework/src/spine.cpp b/core/framework/src/spine.cpp index 53b3b5f..6eb4b51 100644 --- a/core/framework/src/spine.cpp +++ b/core/framework/src/spine.cpp @@ -110,7 +110,11 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; std::shared_ptr command_ping; std::shared_ptr command_ping_result; - command_ping = std::make_shared("kPing", command_map.find("kPing")->second, ""); + command_ping = std::make_shared( + "kPing", + command_map.find("kPing")->second, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString() + ); command_ping_result = std::make_shared( "kPingResult", command_map.find("kPingResult")->second, diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index 70903c2..fce9120 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -141,7 +141,7 @@ namespace rip std::string debugString; // HACK if (device == nullptr) { - throw EmptyDevice(); + throw EmptyDevice("Device is nullptr!"); } if (command->getId() == "") @@ -149,6 +149,11 @@ namespace rip throw EmptyCommand(); } + if ( ! device->isOpen()) + { + throw EmptyDevice("Device not open / available."); + } + std::cout << fmt::format("Command->getEnum(): {} (aka {})", command->getEnum(), command->getId()) << std::endl; // Get the argument types @@ -164,12 +169,13 @@ namespace rip // std::cout << toBytes(command->getEnum()) << std::endl; // Pack the command to send - std::string message = std::to_string(command->getEnum()) + static_cast(m_field_separator); + std::string message = std::to_string(command->getEnum()); std::tuple args_tuple(args...); if (sizeof...(args) > 0) { + message += static_cast(m_field_separator); message += tupleToBytes<0, Args...>(argument_types, args_tuple); message.back() = static_cast(m_command_separator); } @@ -181,23 +187,63 @@ namespace rip // Send the message debugString = byteStringToHexDebugString(message); std::cout << fmt::format("Device->write bytes: {}", debugString) << std::endl; + std::cout << fmt::format("Device baud: {}", device->getBaudrate()) << '\n'; device->write(message); + // device->flushOutput(); + device->flush(); // HACK - device->setTimeout(units::s * 3); + device->setTimeout(units::s * 1); + + std::cout << "reading till I see " << std::string(1, m_command_separator) << std::endl; // Check Acknowledgement - std::string acknowledgement = device->readline(m_max_response_length, std::string(1, m_command_separator)); + // std::string acknowledgement = device->readline(m_max_response_length, ";");//std::string(1, m_command_separator)); + std::string ack_msg, new_data; + uint8_t retry_count = 0; + while (true) + { + new_data = device->read(sizeof(char)); + if (new_data.size() > 1) + { + throw SerialLibrarySucks("cmdmessenger->send(), device->read(): We asked for 1 char but got more than that..."); + } + if (new_data.size() == 0) + { + retry_count++; + if (retry_count > 3) + { + std::cout << "Read retry count exceeded." << '\n'; + break; + } + } + else + { + ack_msg.append(new_data); + // reset the retry whenever we get data back + retry_count = 0; + if (new_data.front() == m_escape_character) + { + // pass the next byte + ack_msg.append(device->read(sizeof(char))); + } + else if (new_data.front() == m_command_separator) + { + // done reading the command + break; + } + } + } - if (acknowledgement.length() == 0) + if (ack_msg.length() == 0) { throw EmptyDeviceResponse(fmt::format("did not receive any bytes from the device, timeout or crash?")); } - debugString = byteStringToHexDebugString(acknowledgement); + debugString = byteStringToHexDebugString(ack_msg); std::cout << fmt::format("Device->readline bytes: {}", debugString) << std::endl; - handleAck(acknowledgement, command); + handleAck(ack_msg, command); m_last_device = device; } @@ -300,7 +346,43 @@ namespace rip } // Unpack the message - std::string response = m_last_device->readline(m_max_response_length, std::string(1, m_command_separator)); + // std::string response = m_last_device->readline(m_max_response_length, std::string(1, m_command_separator)); + std::string response, new_data; + uint8_t retry_count = 0; + while (true) + { + new_data = m_last_device->read(sizeof(char)); + if (new_data.size() > 1) + { + throw SerialLibrarySucks("We asked for 1 char but got more than that..."); + } + if (new_data.size() == 0) + { + retry_count++; + if (retry_count > 3) + { + std::cout << "Read retry count exceeded." << '\n'; + break; + } + } + else + { + response.append(new_data); + // reset the retry whenever we get data back + retry_count = 0; + if (new_data.front() == m_escape_character) + { + // if we have an escape character, go ahead and pass the next byte through + response.append(m_last_device->read(sizeof(char))); + } + else if (new_data.front() == m_command_separator) + { + // and it's not escaped! we must be at the end of the command + break; + } + } + } + std::cout << fmt::format("CmdMessenger->receive(), device->readline() output: {}", byteStringToHexDebugString(response)) << '\n'; if (response.size() == 0) { @@ -318,7 +400,7 @@ namespace rip if (response[0] != m_field_separator) { - throw IncorrectFieldSeparator(); + throw IncorrectFieldSeparator("In CmdMessenger->receive(), after checking response command."); } response.erase(0, 1); @@ -373,7 +455,7 @@ namespace rip } /** - * @brief Convert a type into bytes + * @brief Convert a type into bytes and escape control characters * * @tparam From The current value of the type to convert to bytes * @tparam To The type that is specified for the device @@ -393,30 +475,6 @@ namespace rip template typename std::enable_if::value, std::string>::type toBytes(const From& f) { - /* - To t = static_cast(f); - - if (std::is_same::value) - { - return toBytesString(t); - } - - char* byte_pointer = reinterpret_cast(&t); - std::string rv; - for (size_t i = 0; i < sizeof(t); i++) - { - // Add the escape character - if (*byte_pointer == m_field_separator || - *byte_pointer == m_command_separator || - *byte_pointer == m_escape_character) - { - rv.push_back(static_cast(m_escape_character)); - } - rv.push_back(*byte_pointer); - byte_pointer ++; - } - return rv; - //*/ To t = static_cast(f); if (std::is_same::value) @@ -428,9 +486,17 @@ namespace rip for (auto a_chr : to_bytes(t)) { char n_chr = static_cast(a_chr); - rv += n_chr; + // escape any special control chars here + if ( + n_chr == '\0' + || n_chr == m_field_separator + || n_chr == m_command_separator + ) { + rv += m_escape_character; + } + rv += n_chr; // before or after depending on reversal } - std::reverse(rv.begin(), rv.end()); + // std::reverse(rv.begin(), rv.end()); return rv; } diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp index 3c98006..41e2392 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/exceptions.hpp @@ -39,6 +39,8 @@ namespace rip NEW_EX(UnconvertibleArgument) NEW_EX(UnknownArgument) NEW_EX(DeviceSentErrorResponse) + + NEW_EX(SerialLibrarySucks) } } #endif // CMD_MESSENGER_EXCEPTIONS_HPP From 422b9471a8d3d56045b509393ad3364a7efd8583 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Thu, 29 Mar 2018 18:51:32 -0400 Subject: [PATCH 095/200] Working on stretch support in build --- docker/rip_rpi.dockerfile | 39 +++++++++++++++++++++++++++------------ 1 file changed, 27 insertions(+), 12 deletions(-) diff --git a/docker/rip_rpi.dockerfile b/docker/rip_rpi.dockerfile index c6c2ca0..9400908 100644 --- a/docker/rip_rpi.dockerfile +++ b/docker/rip_rpi.dockerfile @@ -34,12 +34,6 @@ RUN curl -fsSL https://github.com/resin-io-projects/armv7hf-debian-qemu/raw/mast > $SYSROOT/$QEMU_PATH RUN chmod +x $SYSROOT/$QEMU_PATH -ARG pi_image_dist -ENV PI_IMAGE_DIST=${pi_image_dist} -# edit the sources.list for more -RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c "\ - echo \"deb http://archive.raspbian.org/raspbian/ ${PI_IMAGE_DIST} main contrib non-free rpi\" > /etc/apt/sources.list " - RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ cd /dev \ && MAKEDEV -n std \ @@ -47,22 +41,47 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ && MAKEDEV std \ ' +ARG pi_image_dist +ENV PI_IMAGE_DIST=${pi_image_dist} +# edit the sources.list for more +RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c "\ + echo ${PI_IMAGE_DIST} > /etc/pi_image_dist \ + && apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 82B129927FA3303E \ + && echo \"deb http://archive.raspbian.org/raspbian/ ${PI_IMAGE_DIST} main contrib non-free rpi\" > /etc/apt/sources.list \ + && echo \"deb http://archive.raspberrypi.org/debian/ ${PI_IMAGE_DIST} main ui\" >> /etc/apt/sources.list " + +# Run the clean, update, apt-utils, configure, and upgrade RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ apt-get clean \ && apt-get update \ && DEBIAN_FRONTEND=noninteractive apt-get install -y apt-utils \ && DEBIAN_FRONTEND=noninteractive dpkg --configure -a \ && DEBIAN_FRONTEND=noninteractive apt-get upgrade -y ' + +# install the packages which should be available regardless of release version RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ apt-get update \ && DEBIAN_FRONTEND=noninteractive apt-get install -y \ libc6-dev symlinks libssl-dev libcrypto++-dev \ libeigen3-dev libsuitesparse-dev qt5-default \ bash zsh git vim tmux \ - cmake lcov g++ time unzip \ - && symlinks -cors / \ + cmake lcov g++ time unzip ' + +# install the packages only available for certain releases (jessie/stretch/etc) + +# STRETCH requirements +RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ + if grep -Fxq stretch /etc/pi_image_dist; then \ + && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + libzstd-dev \ + ;fi' + +# clean up apt caches and links +RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ + symlinks -cors / \ && apt-get clean' +# G2O RUN mkdir -p $SYSROOT/tmp COPY external/g2o $SYSROOT/tmp/g2o # ARM doesn't support SSE things and g2o's cmake can't detect that because /proc isn't mapped for ARM @@ -75,10 +94,6 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ && make install \ && symlinks -cors /' -# requires a /dev mounted 'mount --bind /dev /rpxc/sysroot/dev' -# mounts require '--cap-add=SYS_ADMIN --security-opt apparmor:unconfined' args to docker run -#RUN install-raspbian python3 - COPY docker/rpi/ / RUN mkdir -p $SYSROOT/workdir WORKDIR /workdir From 08b9390451ecd04d23445486bd459571fb5e7762 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Thu, 29 Mar 2018 20:25:08 -0400 Subject: [PATCH 096/200] Moving apt-key recv --- docker/rip_rpi.dockerfile | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docker/rip_rpi.dockerfile b/docker/rip_rpi.dockerfile index 9400908..e1e5bee 100644 --- a/docker/rip_rpi.dockerfile +++ b/docker/rip_rpi.dockerfile @@ -46,15 +46,15 @@ ENV PI_IMAGE_DIST=${pi_image_dist} # edit the sources.list for more RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c "\ echo ${PI_IMAGE_DIST} > /etc/pi_image_dist \ - && apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 82B129927FA3303E \ && echo \"deb http://archive.raspbian.org/raspbian/ ${PI_IMAGE_DIST} main contrib non-free rpi\" > /etc/apt/sources.list \ && echo \"deb http://archive.raspberrypi.org/debian/ ${PI_IMAGE_DIST} main ui\" >> /etc/apt/sources.list " -# Run the clean, update, apt-utils, configure, and upgrade +# Run the clean, update, apt needs, configure, and upgrade RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ apt-get clean \ && apt-get update \ - && DEBIAN_FRONTEND=noninteractive apt-get install -y apt-utils \ + && DEBIAN_FRONTEND=noninteractive apt-get install -y apt-utils dirmngr \ + && apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 82B129927FA3303E \ && DEBIAN_FRONTEND=noninteractive dpkg --configure -a \ && DEBIAN_FRONTEND=noninteractive apt-get upgrade -y ' From 8570c04b6744e01738b1d15daee47c2eb892ae20 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 01:28:26 -0400 Subject: [PATCH 097/200] Stretch builds should soon be the default? --- docker/rip_rpi.dockerfile | 3 ++- go-docker.sh | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/docker/rip_rpi.dockerfile b/docker/rip_rpi.dockerfile index e1e5bee..1218aad 100644 --- a/docker/rip_rpi.dockerfile +++ b/docker/rip_rpi.dockerfile @@ -55,6 +55,7 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ && apt-get update \ && DEBIAN_FRONTEND=noninteractive apt-get install -y apt-utils dirmngr \ && apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 82B129927FA3303E \ + && apt-get update \ && DEBIAN_FRONTEND=noninteractive dpkg --configure -a \ && DEBIAN_FRONTEND=noninteractive apt-get upgrade -y ' @@ -72,7 +73,7 @@ RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ # STRETCH requirements RUN chroot $SYSROOT $QEMU_PATH /bin/sh -c '\ if grep -Fxq stretch /etc/pi_image_dist; then \ - && DEBIAN_FRONTEND=noninteractive apt-get install -y \ + DEBIAN_FRONTEND=noninteractive apt-get install -y \ libzstd-dev \ ;fi' diff --git a/go-docker.sh b/go-docker.sh index f8e8a45..c17f8cf 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -61,7 +61,7 @@ function build_rip_deps() { function build_rip_rpi() { if ($PROMPTER --title "Comfirm Build?" --yesno "Do not run this command unless you were told to do so. Requires files outside of RIP." 8 68) then if find ../rpi_images -prune -empty ; then - rpi_image_file="$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | sort -g | tail -1 )" + rpi_image_file=${rpi_image_file:-"$(find ../rpi_images -mindepth 1 -maxdepth 1 -printf '%f\n' | sort -g | tail -1 )"} echo "Found rpi debootstrap image ${rpi_image_file}" rsync --copy-links --times ../rpi_images/${rpi_image_file} ./external/rpi_images/ if [ -z "$RPI_DOCKER_TAG" ]; then From e5b6ac62c47822de56a16d19511e581558ae858a Mon Sep 17 00:00:00 2001 From: Parker Mitchell Date: Fri, 30 Mar 2018 13:57:34 -0400 Subject: [PATCH 098/200] Fix gcc 6.3 arm notes for JSON by adding compile flag --- CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index fcea426..c4aaf72 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,8 @@ endif() # downstream CMake files. set (CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-psabi") + option(GCC_DIAG_COLORS "Enable GCC Diagnostic Colors" ON) if (GCC_DIAG_COLORS) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always") From 3049f15c6c2f5d2c58ccb90b89e535b4826edacb Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 17:16:25 -0400 Subject: [PATCH 099/200] Add an option for image tag --- go-docker.sh | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/go-docker.sh b/go-docker.sh index c17f8cf..c1bd24a 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -23,6 +23,14 @@ while [[ "$1" != "" ]] ; do exit 1 fi ;; + --rpxc-tag) + if [[ "$1" != "" ]]; then + RPXC_IMAGE_TAG="$1" + else + echo " --rpxc-tag requires a docker tag id" + exit 1 + fi + ;; '--') shift if [[ "$1" != "" ]]; then From c9c37d2a6d4e16e86d4de8a8a333a0dbfd0aaf27 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 17:17:19 -0400 Subject: [PATCH 100/200] Cleaning up the logging and couts --- appendages/src/roboclaw.cpp | 24 +++++++++++++++--- .../include/cmd_messenger/cmd_messenger.hpp | 25 ++++++++++--------- .../cmd_messenger/test/mock_device.hpp | 7 ++++++ 3 files changed, 41 insertions(+), 15 deletions(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 4af880a..9185ad7 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -70,7 +70,19 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>() ) ), - m_set_velocity_pid(createCommand("kSetVelocityPID", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())) + m_set_velocity_pid( + createCommand( + "kSetVelocityPID", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::CharType, + cmdmessenger::ArduinoCmdMessenger::FloatType, + cmdmessenger::ArduinoCmdMessenger::FloatType, + cmdmessenger::ArduinoCmdMessenger::FloatType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>() + ) + ) { if (config.find("address") == config.end()) { @@ -135,6 +147,7 @@ namespace rip } void Roboclaw::stop() { + misc::Logger::getInstance()->debug("Setting roboclaw duty to zero."); SetDuty(0, 0); } @@ -143,23 +156,28 @@ namespace rip std::chrono::time_point start_time = std::chrono::system_clock::now(); misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start"); - misc::Logger::getInstance()->debug("Read encoders for 10s in a loop"); - while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) + misc::Logger::getInstance()->debug("Read encoders for 5s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) { misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); } misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); SetDuty(16000, 16000); start_time = std::chrono::system_clock::now(); while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) {} + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); stop(); misc::Logger::getInstance()->debug("Setting speed accel drive (5s)"); + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); SetSpeedAccel(12000, 12000, 12000); start_time = std::chrono::system_clock::now(); while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) {} + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); stop(); + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); } } } diff --git a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp index fce9120..c44cb29 100644 --- a/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp +++ b/core/utilities/cmd_messenger/include/cmd_messenger/cmd_messenger.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include "command.hpp" @@ -154,8 +155,6 @@ namespace rip throw EmptyDevice("Device not open / available."); } - std::cout << fmt::format("Command->getEnum(): {} (aka {})", command->getEnum(), command->getId()) << std::endl; - // Get the argument types std::string argument_types = command->getArgumentTypes(); const std::size_t value = sizeof...(Args); @@ -165,7 +164,8 @@ namespace rip } debugString = byteStringToHexDebugString(std::to_string(command->getEnum())); - std::cout << fmt::format("to_str(command->getEnum()) bytes: {}", debugString) << std::endl; + // std::cout << fmt::format("to_str(command->getEnum()) bytes: {}", debugString) << std::endl; + misc::Logger::getInstance()->debug(fmt::format("CmdMessenger sending: '{}:{}' as '{}')", command->getEnum(), command->getId(), debugString )); // std::cout << toBytes(command->getEnum()) << std::endl; // Pack the command to send @@ -187,7 +187,7 @@ namespace rip // Send the message debugString = byteStringToHexDebugString(message); std::cout << fmt::format("Device->write bytes: {}", debugString) << std::endl; - std::cout << fmt::format("Device baud: {}", device->getBaudrate()) << '\n'; + // std::cout << fmt::format("Device baud: {}", device->getBaudrate()) << '\n'; device->write(message); // device->flushOutput(); device->flush(); @@ -195,7 +195,7 @@ namespace rip // HACK device->setTimeout(units::s * 1); - std::cout << "reading till I see " << std::string(1, m_command_separator) << std::endl; + // std::cout << "reading till I see " << std::string(1, m_command_separator) << std::endl; // Check Acknowledgement // std::string acknowledgement = device->readline(m_max_response_length, ";");//std::string(1, m_command_separator)); @@ -261,7 +261,7 @@ namespace rip void handleAck(std::string& acknowledgement, std::shared_ptr command) { std::string debugString = byteStringToHexDebugString(acknowledgement); - std::cout << fmt::format("Ack Str: {}", debugString) << std::endl; + // std::cout << fmt::format("Ack Str: {}", debugString) << std::endl; // First part should be the acknowledgment id // (a two-byte integer by default) @@ -271,6 +271,7 @@ namespace rip acknowledgement.erase(0, acknowledgement.find(m_field_separator)); // kAcknowledge will always be zero + // kError will always be 1 if (acknowledgement_id == 1) { debugString = byteStringToHexDebugString(acknowledgement); @@ -282,18 +283,18 @@ namespace rip } // XXX - debugString = byteStringToHexDebugString(acknowledgement); - std::cout << fmt::format("After checking ack_id: {}", debugString) << std::endl; + // debugString = byteStringToHexDebugString(acknowledgement); + // std::cout << fmt::format("After checking ack_id: {}", debugString) << std::endl; // Then the field separator if (acknowledgement[0] != m_field_separator) { - std::cout << fmt::format("Bad field_separator: Wanted '{}' but got '{}' !", m_field_separator, acknowledgement[0]) << std::endl; + std::cerr << fmt::format("Bad field_separator: Wanted '{}' but got '{}' !", m_field_separator, acknowledgement[0]) << std::endl; throw IncorrectFieldSeparator(fmt::format("Wanted '{}' but got '{}' !", m_field_separator, acknowledgement[0])); } acknowledgement.erase(0, 1); - debugString = byteStringToHexDebugString(acknowledgement); - std::cout << fmt::format("After checking field_separator: {}", debugString) << std::endl; + // debugString = byteStringToHexDebugString(acknowledgement); + // std::cout << fmt::format("After checking field_separator: {}", debugString) << std::endl; // Then the command sent T_IntegerType acknowledge_command = fromBytes(acknowledgement); @@ -309,7 +310,7 @@ namespace rip throw IncorrectCommandSeparator(fmt::format("Wanted '{}' but got '{}' !", m_command_separator, acknowledgement[0])); } - std::cout << fmt::format("Successfully acknowledged command {}.", command->getId()) << std::endl; + misc::Logger::getInstance()->debug(fmt::format("CmdMessenger: Successfully acknowledged command {}.", command->getId() )); } /** diff --git a/core/utilities/cmd_messenger/test/mock_device.hpp b/core/utilities/cmd_messenger/test/mock_device.hpp index 4fb0b5a..f899163 100644 --- a/core/utilities/cmd_messenger/test/mock_device.hpp +++ b/core/utilities/cmd_messenger/test/mock_device.hpp @@ -16,6 +16,11 @@ namespace rip public: MockDevice(); + bool isOpen () const + { + return true; + } + /** * @brief write * @param message @@ -156,10 +161,12 @@ namespace rip return rv; } + private: std::string m_response; std::string m_last_sent; bool m_correctly_acknowledge; + bool m_self_is_open; char m_field_separator; char m_command_separator; char m_escape_character; From a19deae77f3d1fa971060b0f062502c6dda633be Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 17:48:58 -0400 Subject: [PATCH 101/200] Missed a shift --- go-docker.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/go-docker.sh b/go-docker.sh index c1bd24a..053f5f5 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -24,6 +24,7 @@ while [[ "$1" != "" ]] ; do fi ;; --rpxc-tag) + shift if [[ "$1" != "" ]]; then RPXC_IMAGE_TAG="$1" else From cbf86333be4db2c6a83445d5d912aa7ae479e2ef Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 17:51:35 -0400 Subject: [PATCH 102/200] reload conf when changed --- go-docker.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/go-docker.sh b/go-docker.sh index 053f5f5..237886f 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -27,6 +27,7 @@ while [[ "$1" != "" ]] ; do shift if [[ "$1" != "" ]]; then RPXC_IMAGE_TAG="$1" + source docker/core.sh else echo " --rpxc-tag requires a docker tag id" exit 1 From c24211226979a12a1d032a28e326eb8aa773e953 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 17:56:30 -0400 Subject: [PATCH 103/200] Gosh darn envvars --- go-docker.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/go-docker.sh b/go-docker.sh index 237886f..559452b 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -27,6 +27,7 @@ while [[ "$1" != "" ]] ; do shift if [[ "$1" != "" ]]; then RPXC_IMAGE_TAG="$1" + unset RPXC_IMAGE source docker/core.sh else echo " --rpxc-tag requires a docker tag id" From 5059aa168976c37586e476888827fa0ae213ed01 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 17:56:30 -0400 Subject: [PATCH 104/200] Gosh darn envvars --- go-docker.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/go-docker.sh b/go-docker.sh index 237886f..559452b 100755 --- a/go-docker.sh +++ b/go-docker.sh @@ -27,6 +27,7 @@ while [[ "$1" != "" ]] ; do shift if [[ "$1" != "" ]]; then RPXC_IMAGE_TAG="$1" + unset RPXC_IMAGE source docker/core.sh else echo " --rpxc-tag requires a docker tag id" From 9d5cdb251d3efe4b3098e467e807979476c5b354 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 18:35:44 -0400 Subject: [PATCH 105/200] Force enums to 16-bit --- arduino_gen/code_template.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/arduino_gen/code_template.txt b/arduino_gen/code_template.txt index 5d810af..45b0607 100644 --- a/arduino_gen/code_template.txt +++ b/arduino_gen/code_template.txt @@ -10,7 +10,7 @@ const char LED = 13; {constructors} -enum +enum RIPenum : int16_t {{ {command_enums} }}; From ae109ae9356b6fe2dcdaae30a010213a6d1f8750 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Fri, 30 Mar 2018 20:52:38 -0400 Subject: [PATCH 106/200] Working on bad ultrasonic reads --- appendages/json/ultrasonic.json | 3 ++- appendages/src/ultrasonic.cpp | 6 +++--- appendages/xml/roboclaw.xml | 8 ++++---- appendages/xml/ultrasonic.xml | 12 ++++++++++++ arduino_gen/src/argument.cpp | 3 ++- 5 files changed, 23 insertions(+), 9 deletions(-) diff --git a/appendages/json/ultrasonic.json b/appendages/json/ultrasonic.json index 1367142..111bc0c 100644 --- a/appendages/json/ultrasonic.json +++ b/appendages/json/ultrasonic.json @@ -1,4 +1,5 @@ { "triggerPin": "int", - "echoPin": "int" + "echoPin": "int", + "minSleepMs": "int" } diff --git a/appendages/src/ultrasonic.cpp b/appendages/src/ultrasonic.cpp index 4976b38..cdcfe8e 100644 --- a/appendages/src/ultrasonic.cpp +++ b/appendages/src/ultrasonic.cpp @@ -36,14 +36,14 @@ namespace rip bool Ultrasonic::diagnostic() { std::chrono::time_point start_time = std::chrono::system_clock::now(); - misc::Logger::getInstance()->debug("Reading the ultrasonic value for 10s."); + misc::Logger::getInstance()->info("Reading the ultrasonic value for 10s."); while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) { - misc::Logger::getInstance()->debug(fmt::format("Distance: {} cm", read().to(units::cm))); + misc::Logger::getInstance()->info(fmt::format("Distance: {} cm", read().to(units::cm))); } - misc::Logger::getInstance()->debug("Ultrasonic diag finished."); + misc::Logger::getInstance()->info("Ultrasonic diag finished."); return true; } diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 96c6936..eda1329 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -4,8 +4,8 @@ - - + + @@ -66,7 +66,7 @@ bool result; - + switch (motor) { case 1: @@ -79,7 +79,7 @@ result = false; break; } - + if (!result) // TODO: Double check return values { cmdMessenger.sendBinCmd(kError, kSetVelocityPID); diff --git a/appendages/xml/ultrasonic.xml b/appendages/xml/ultrasonic.xml index 52d927b..9ffd44f 100644 --- a/appendages/xml/ultrasonic.xml +++ b/appendages/xml/ultrasonic.xml @@ -8,6 +8,12 @@ + + + + + + // Ultrasonic triggerPin: $triggerPin$ @@ -19,7 +25,13 @@ + unsigned long curtime = millis(); + if (curtime < sonar_prevtime[indexNum]+sonar_timeouts[indexNum]) + { + delay(sonar_prevtime[indexNum]+sonar_timeouts[indexNum] - curtime ); + } rv = sonar[indexNum].ping_cm(); + sonar_prevtime[indexNum] = millis(); diff --git a/arduino_gen/src/argument.cpp b/arduino_gen/src/argument.cpp index f975af2..38f672a 100644 --- a/arduino_gen/src/argument.cpp +++ b/arduino_gen/src/argument.cpp @@ -50,7 +50,8 @@ namespace rip m_type != "int" && m_type != "bool" && m_type != "string" && - m_type != "string_literal") + m_type != "long" && + m_type != "string_literal") { throw AttributeException(fmt::format("Constructor argument unknown type on line number {}", xml->GetLineNum())); From c5f1fbd94712eff4ce25eebaf1c1bc25fb04f229 Mon Sep 17 00:00:00 2001 From: Aiden Date: Sat, 31 Mar 2018 10:59:56 -0400 Subject: [PATCH 107/200] Added respect buffer flag --- appendages/include/appendages/roboclaw.hpp | 22 ++-- appendages/src/roboclaw.cpp | 126 +++++++++++++-------- appendages/xml/roboclaw.xml | 43 ++++--- 3 files changed, 121 insertions(+), 70 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index e5ede7c..90fa99f 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -27,18 +27,18 @@ namespace rip void setM2SpeedAccel(uint32_t accel, int32_t speed); void setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2); - void setM1SpeedDist(int32_t speed, uint32_t distance); - void setM2SpeedDist(int32_t speed, uint32_t distance); - void setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2); + void setM1SpeedDist(int32_t speed, uint32_t distance, uint8_t flag=1); + void setM2SpeedDist(int32_t speed, uint32_t distance, uint8_t flag=1); + void setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2, uint8_t flag=1); - void setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance); - void setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance); - void setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2); + void setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, uint8_t flag=1); + void setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, uint8_t flag=1); + void setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2, uint8_t flag=1); - void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, uint32_t position); - void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position); + void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, uint32_t position, uint8_t flag=1); + void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position, uint8_t flag=1); void setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, - uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2); + uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2, uint8_t flag=1); int32_t readM1Encoder(); int32_t readM2Encoder(); @@ -62,8 +62,8 @@ namespace rip void setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps); void setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps); - void setDynamics(bool motor, const MotorDynamics& dynamics); //Not implemented on arduino - void setDynamics(const MotorDynamics& dynamics); //Not implemented on arduino + void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=1); //Not implemented on arduino + void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=1); //Not implemented on arduino /** * Stop! ^0^ */ diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index b199e15..af4a428 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -48,30 +48,35 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m2_speed_dist(createCommand("kSetM2SpeedDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m1m2_speed_dist(createCommand("kSetM1M2SpeedDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m1_speed_accel_dist(createCommand("kSetM1SpeedAccelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m2_speed_accel_dist(createCommand("kSetM2SpeedAccelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m1m2_speed_accel_dist(createCommand("kSetM1M2SpeedAccelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m1_speed_accel_decel_dist(createCommand("kSetM1SpeedAccelDecelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m2_speed_accel_decel_dist(createCommand("kSetM2SpeedAccelDecelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m1m2_speed_accel_decel_dist(createCommand("kSetM1M2SpeedAccelDecelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>())), m_read_m1_encoder(createCommand("kReadM1Encoder", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), @@ -198,7 +207,8 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1_speed, m_id, m_address, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_speed, m_id, + m_address, speed); } void Roboclaw::setM2Speed(int32_t speed) @@ -206,7 +216,8 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m2_speed, m_id, m_address, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed, m_id, + m_address, speed); } void Roboclaw::setM1M2Speed(int32_t speed1, int32_t speed2) @@ -215,7 +226,8 @@ namespace rip messenger.send(m_device, m_set_m1m2_speed, m_id, m_address, speed1, speed2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed, m_id, + m_address, speed1, speed2); } void Roboclaw::setM1SpeedAccel(uint32_t accel, int32_t speed) @@ -224,7 +236,8 @@ namespace rip messenger.send(m_device, m_set_m1_speed_accel, m_id, m_address, accel, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_speed_accel, m_id, + m_address, accel, speed); } void Roboclaw::setM2SpeedAccel(uint32_t accel, int32_t speed) @@ -233,7 +246,8 @@ namespace rip messenger.send(m_device, m_set_m2_speed_accel, m_id, m_address, accel, speed); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_speed_accel, m_id, + m_address, accel, speed); } void Roboclaw::setM1M2SpeedAccel(uint32_t accel, int32_t speed1, int32_t speed2) @@ -243,28 +257,34 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::CharType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, - cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed_accel, m_id, m_address, accel, speed1, speed2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1m2_speed_accel, m_id, + m_address, accel, speed1, speed2); } - void Roboclaw::setM1SpeedDist(int32_t speed, uint32_t distance) + void Roboclaw::setM1SpeedDist(int32_t speed, uint32_t distance, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1_speed_dist, m_id, m_address, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1_speed_dist, m_id, + m_address, speed, distance, flag); } - void Roboclaw::setM2SpeedDist(int32_t speed, uint32_t distance) + void Roboclaw::setM2SpeedDist(int32_t speed, uint32_t distance, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m2_speed_dist, m_id, m_address, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m2_speed_dist, m_id, + m_address, speed, distance, flag); } - void Roboclaw::setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) + void Roboclaw::setM1M2SpeedDist(int32_t speed1, uint32_t distance1, int32_t speed2, + uint32_t distance2, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1m2_speed_dist, m_id, m_address, speed1, distance1, speed2, distance2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1m2_speed_dist, m_id, + m_address, speed1, distance1, speed2, distance2, flag); } - void Roboclaw::setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) + void Roboclaw::setM1SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1_speed_accel_dist, m_id, m_address, accel, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1_speed_accel_dist, m_id, + m_address, accel, speed, distance, flag); } - void Roboclaw::setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance) + void Roboclaw::setM2SpeedAccelDist(uint32_t accel, int32_t speed, uint32_t distance, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m2_speed_accel_dist, m_id, m_address, accel, speed, distance); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m2_speed_accel_dist, m_id, + m_address, accel, speed, distance, flag); } - void Roboclaw::setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, int32_t speed2, uint32_t distance2) + void Roboclaw::setM1M2SpeedAccelDist(uint32_t accel, int32_t speed1, uint32_t distance1, + int32_t speed2, uint32_t distance2, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1m2_speed_accel_dist, m_id, m_address, accel, speed1, distance1, speed2, distance2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1m2_speed_accel_dist, m_id, + m_address, accel, speed1, distance1, speed2, distance2, flag); } - void Roboclaw::setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) + void Roboclaw::setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, + uint32_t position, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1_speed_accel_decel_dist, m_id, m_address, accel, speed, deccel, position); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1_speed_accel_decel_dist, m_id, + m_address, accel, speed, deccel, position, flag); } - void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position) + void Roboclaw::setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, + uint32_t position, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m2_speed_accel_decel_dist, m_id, m_address, accel, speed, deccel, position); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m2_speed_accel_decel_dist, m_id, + m_address, accel, speed, deccel, position, flag); } void Roboclaw::setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, - uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2) + uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; messenger.send( - m_device, m_set_m1m2_speed_accel_decel_dist, m_id, m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1m2_speed_accel_decel_dist, m_id, + m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, flag); } int32_t Roboclaw::readM1Encoder() @@ -456,7 +492,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::CharType>(m_get_buffers_result); } - void Roboclaw::setDynamics(bool motor, const MotorDynamics& dynamics) + void Roboclaw::setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer) { cmdmessenger::ArduinoCmdMessenger messenger; int32_t speed; @@ -501,11 +537,11 @@ namespace rip dist = static_cast((*dynamics.getDistance() / (m_wheel_radius * M_PI * 2)).to(units::none) * m_ticks_per_rev); if(motor) { - setM2SpeedDist(speed, dist); + setM2SpeedDist(speed, dist, static_cast(respectBuffer)); } else { - setM1SpeedDist(speed, dist); + setM1SpeedDist(speed, dist, static_cast(respectBuffer)); } return; } @@ -516,11 +552,11 @@ namespace rip accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); if(motor) { - setM2SpeedAccelDist(accel, speed, dist); + setM2SpeedAccelDist(accel, speed, dist, static_cast(respectBuffer)); } else { - setM1SpeedAccelDist(accel, speed, dist); + setM1SpeedAccelDist(accel, speed, dist, static_cast(respectBuffer)); } return; } @@ -532,11 +568,11 @@ namespace rip decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); if(motor) { - setM2SpeedAccelDecelDist(accel, speed, decel, dist); + setM2SpeedAccelDecelDist(accel, speed, decel, dist, static_cast(respectBuffer)); } else { - setM1SpeedAccelDecelDist(accel, speed, decel, dist); + setM1SpeedAccelDecelDist(accel, speed, decel, dist, static_cast(respectBuffer)); } return; } @@ -547,7 +583,7 @@ namespace rip } } - void Roboclaw::setDynamics(const MotorDynamics& dynamics) + void Roboclaw::setDynamics(const MotorDynamics& dynamics, bool respectBuffer) { cmdmessenger::ArduinoCmdMessenger messenger; int32_t speed; @@ -576,7 +612,7 @@ namespace rip { speed = static_cast((*dynamics.getSpeed() / (m_wheel_radius * M_PI * 2)).to(1 / units::s) * m_ticks_per_rev); dist = static_cast((*dynamics.getDistance() / (m_wheel_radius * M_PI * 2)).to(units::none) * m_ticks_per_rev); - setM1M2SpeedDist(speed, dist, speed, dist); + setM1M2SpeedDist(speed, dist, speed, dist, static_cast(respectBuffer)); return; } case MotorDynamics::DType::kSpeedAccelDist: @@ -584,7 +620,7 @@ namespace rip speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); - setM1M2SpeedAccelDist(accel, speed, dist, speed, dist); + setM1M2SpeedAccelDist(accel, speed, dist, speed, dist, static_cast(respectBuffer)); return; } case MotorDynamics::DType::kSpeedAccelDecelDist: @@ -593,7 +629,7 @@ namespace rip dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); - setM1M2SpeedAccelDecelDist(accel, speed, decel, dist, accel, speed, decel, dist); + setM1M2SpeedAccelDecelDist(accel, speed, decel, dist, accel, speed, decel, dist, static_cast(respectBuffer)); return; } default: diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 7c38344..3315b43 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -86,8 +86,9 @@ + - if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance)) + if (!roboclaws[indexNum].SpeedDistanceM1(address, speed, distance, flag)) { cmdMessenger.sendBinCmd(kError, kSetM1SpeedDist); } @@ -97,8 +98,9 @@ + - if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance)) + if (!roboclaws[indexNum].SpeedDistanceM2(address, speed, distance, flag)) { cmdMessenger.sendBinCmd(kError, kSetM2SpeedDist); } @@ -110,8 +112,9 @@ + - if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2)) + if (!roboclaws[indexNum].SpeedDistanceM1M2(address, speed1, distance1, speed2, distance2, flag)) { cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedDist); } @@ -124,8 +127,9 @@ + - if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance)) + if (!roboclaws[indexNum].SpeedAccelDistanceM1(address, accel, speed, distance, flag)) { cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccelDist); } @@ -136,8 +140,9 @@ + - if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance)) \ + if (!roboclaws[indexNum].SpeedAccelDistanceM2(address, accel, speed, distance, flag)) { cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDist); } @@ -150,23 +155,25 @@ + - if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2)) + if (!roboclaws[indexNum].SpeedAccelDistanceM1M2(address, accel, speed1, distance1, speed2, distance2, flag)) { cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccelDist); } - + + - if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position)) + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1(address, accel, speed, deccel, position, flag)) { cmdMessenger.sendBinCmd(kError, kSetM1SpeedAccelDecelDist); } @@ -178,8 +185,9 @@ + - if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position)) + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM2(address, accel, speed, deccel, position, flag)) { cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDecelDist); } @@ -195,8 +203,9 @@ + - if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2)) + if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, flag)) { cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDecelDist); } @@ -245,7 +254,9 @@ - enc1Speed = roboclaws[indexNum].ReadSpeedM1(address, status, valid); + bool valid; + uint8_t status; + enc1Speed = roboclaws[indexNum].ReadSpeedM1(address, &status, &valid); if (!valid) { cmdMessenger.sendBinCmd(kError, kReadM1EncoderSpeed); @@ -257,7 +268,9 @@ - enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, status, valid); + bool valid; + uint8_t status; + enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, &status, &valid); if (!valid) { cmdMessenger.sendBinCmd(kError, kReadM2EncoderSpeed); @@ -269,8 +282,10 @@ - enc1Speed = roboclaws[indexNum].ReadSpeedM2(address, status, validM1); - enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, status, validM2); + bool validM1, validM2; + uint8_t status; + enc1Speed = roboclaws[indexNum].ReadSpeedM2(address, &status, &validM1); + enc2Speed = roboclaws[indexNum].ReadSpeedM2(address, &status, &validM2); if (!validM1 || !validM2) { From 98d813d8e28fcaa4338e9b092555ca98f8eadfcf Mon Sep 17 00:00:00 2001 From: Aiden Date: Sat, 31 Mar 2018 12:37:33 -0400 Subject: [PATCH 108/200] new diag --- appendages/src/roboclaw.cpp | 138 ++++++++++++++++++++++++++++++++---- 1 file changed, 123 insertions(+), 15 deletions(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index af4a428..40108c4 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -644,7 +644,8 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m1_duty, m_id, m_address, duty); + cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>(m_device, m_set_m1_duty, m_id, + m_address, duty); } void Roboclaw::setM2Duty(int16_t duty) @@ -652,7 +653,8 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_set_m2_duty, m_id, m_address, duty); + cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>(m_device, m_set_m2_duty, m_id, + m_address, duty); } void Roboclaw::setM1M2Duty(int16_t duty1, int16_t duty2) @@ -661,7 +663,8 @@ namespace rip messenger.send(m_device, m_set_m1m2_duty, m_id, m_address, duty1, duty2); + cmdmessenger::ArduinoCmdMessenger::UnsignedIntegerType>(m_device, m_set_m1m2_duty, m_id, + m_address, duty1, duty2); } void Roboclaw::setM1VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) @@ -672,7 +675,8 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::FloatType, - cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_velocity_pid, m_id, m_address, Kp, Ki, Kd, qpps); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m1_velocity_pid, m_id, + m_address, Kp, Ki, Kd, qpps); } void Roboclaw::setM2VelocityPID(float Kp, float Ki, float Kd, uint32_t qpps) @@ -683,7 +687,8 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::FloatType, cmdmessenger::ArduinoCmdMessenger::FloatType, - cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_velocity_pid, m_id, m_address, Kp, Ki, Kd, qpps); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_device, m_set_m2_velocity_pid, m_id, + m_address, Kp, Ki, Kd, qpps); } void Roboclaw::stop() @@ -693,27 +698,130 @@ namespace rip bool Roboclaw::diagnostic() { - /* std::chrono::time_point start_time = std::chrono::system_clock::now(); misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start"); + //encoder reading diag + misc::Logger::getInstance()->debug("Read encoders for 5s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks(cm): {} | Encoder 2 Ticks(cm): {}", (readEncoders()[0]).to(units::cm), (readEncoders()[1]).to(units::cm))); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + misc::Logger::getInstance()->debug(fmt::format("Encoder 1 velocity(cm/s): {} | Encoder 2 velocity(cm/s): {}", + (readEncoderSpeeds()[0]).to(units::cm / units::s), (readEncoderSpeeds()[1]).to(units::cm / units::s))); - misc::Logger::getInstance()->debug("Read encoders for 10s in a loop"); - while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 10000) + } + start_time = std::chrono::system_clock::now(); + //buffer reading diag + misc::Logger::getInstance()->debug("Getting buffers for 2s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 2000) { - misc::Logger::getInstance()->debug(fmt::format("Encoder 1 Ticks: {} | Encoder 2 Ticks: {}", std::get<0>(ReadEncoders()), std::get<1>(ReadEncoders()))); + misc::Logger::getInstance()->debug(fmt::format("Buffer M1: {} M2: {}", std::get<0>(getBuffers()), std::get<1>(getBuffers()))); + } + start_time = std::chrono::system_clock::now(); + //encoder resetting diag + misc::Logger::getInstance()->debug("Resetting encoders for 2s in a loop"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 2000) + { + resetEncoders(); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); } - misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); - SetDuty(16000, 16000); start_time = std::chrono::system_clock::now(); + //duty setting diag + misc::Logger::getInstance()->debug("Setting Duty to ~1/2 Power, forward for 5 seconds"); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) - {} + { + setM1M2Duty(16000, 16000); + setM1Duty(16000); + setM2Duty(16000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } stop(); + //speed accel drive diag misc::Logger::getInstance()->debug("Setting speed accel drive (5s)"); - SetSpeedAccel(12000, 12000, 12000); + start_time = std::chrono::system_clock::now(); + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) - {} + { + setM1M2SpeedAccel(12000, 12000, 12000); + setM1SpeedAccel(12000, 12000); + setM2SpeedAccel(12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } stop(); - */ + misc::Logger::getInstance()->debug("Setting speed drive (5s)"); + start_time = std::chrono::system_clock::now(); + //speed drive diag + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1Speed(12000); + setM2Speed(12000); + setM1M2Speed(12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //speed dist drive diag + misc::Logger::getInstance()->debug("Setting speed dist drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1SpeedDist(12000, 12000); + setM2SpeedDist(12000, 12000); + setM1M2SpeedDist(12000, 12000, 12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //speed accel dist drive diag + misc::Logger::getInstance()->debug("Setting speed accel dist drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1SpeedAccelDist(12000, 12000, 12000); + setM2SpeedAccelDist(12000, 12000, 12000); + setM1M2SpeedAccelDist(12000, 12000, 12000, 12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //speed accel decel dist drive diag + misc::Logger::getInstance()->debug("Setting speed accel decel dist drive (5s)"); + start_time = std::chrono::system_clock::now(); + + while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) + { + setM1SpeedAccelDecelDist(12000, 12000, 12000, 12000); + setM2SpeedAccelDecelDist(12000, 12000, 12000, 12000); + setM1M2SpeedAccelDecelDist(12000, 12000, 12000, 12000, 12000, 12000, 12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); + misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); + } + stop(); + //setDynamics diag + misc::Logger::getInstance()->debug("Setting via setDynamics"); + + MotorDynamics dynamics; + dynamics.setSpeed(.5 * units::ft / units::s); + setDynamics(0, dynamics); + setDynamics(1, dynamics); + setDynamics(dynamics); + + dynamics.setDistance(1 * units::ft); + setDynamics(dynamics); + + dynamics.setAcceleration(2 * units::ft / (units::s * units::s)); + setDynamics(dynamics); + stop(); + misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics complete"); + + return 0; + } } } From b103bd57d8e8a14a94ac41a9377fa2762db40fcd Mon Sep 17 00:00:00 2001 From: Aiden Date: Sat, 31 Mar 2018 16:46:00 -0400 Subject: [PATCH 109/200] Encoder result command fix --- appendages/include/appendages/roboclaw.hpp | 10 +++++--- appendages/src/roboclaw.cpp | 30 ++++++++++++++-------- 2 files changed, 25 insertions(+), 15 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 90fa99f..238dddd 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -120,14 +120,16 @@ namespace rip std::shared_ptr m_read_m1_encoder; std::shared_ptr m_read_m2_encoder; std::shared_ptr m_read_m1m2_encoder; - std::shared_ptr m_read_encoder_result; - std::shared_ptr m_read_encoders_result; + std::shared_ptr m_read_m1_encoder_result; + std::shared_ptr m_read_m2_encoder_result; + std::shared_ptr m_read_m1m2_encoder_result; std::shared_ptr m_read_m1_encoder_speed; std::shared_ptr m_read_m2_encoder_speed; std::shared_ptr m_read_m1m2_encoder_speed; - std::shared_ptr m_read_encoder_speed_result; - std::shared_ptr m_read_encoders_speed_result; + std::shared_ptr m_read_m1_encoder_speed_result; + std::shared_ptr m_read_m2_encoder_speed_result; + std::shared_ptr m_read_m1m2_encoder_speed_result; std::shared_ptr m_reset_encoders; std::shared_ptr m_get_buffers; diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 40108c4..86c5fb1 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -123,9 +123,11 @@ namespace rip m_read_m1m2_encoder(createCommand("kReadM1M2Encoder", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoder_result(createCommand("kReadEncoderResult", command_map, + m_read_m1_encoder_result(createCommand("kReadM1EncoderResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoders_result(createCommand("kReadEncodersResult", command_map, + m_read_m2_encoder_result(createCommand("kReadM2EncoderResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m1m2_encoder_result(createCommand("kReadM1M2EncoderResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_read_m1_encoder_speed(createCommand("kReadM1EncoderSpeed", command_map, @@ -137,9 +139,11 @@ namespace rip m_read_m1m2_encoder_speed(createCommand("kReadM1M2EncoderSpeed", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoder_speed_result(createCommand("kReadEncoderSpeedResult", command_map, + m_read_m1_encoder_speed_result(createCommand("kReadM1EncoderSpeedResult", command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), + m_read_m2_encoder_speed_result(createCommand("kReadM2EncoderSpeedResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), - m_read_encoders_speed_result(createCommand("kReadEncodersSpeedResult", command_map, + m_read_m1m2_encoder_speed_result(createCommand("kReadM1M2EncoderSpeedResult", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString())), m_reset_encoders(createCommand("kResetEncoders", command_map, @@ -383,7 +387,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read_m1_encoder, m_id, m_address); - return std::get<0>(messenger.receive(m_read_encoder_result)); + return std::get<0>(messenger.receive(m_read_m1_encoder_result)); } int32_t Roboclaw::readM2Encoder() @@ -391,7 +395,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read_m2_encoder, m_id, m_address); - return std::get<0>(messenger.receive(m_read_encoder_result)); + return std::get<0>(messenger.receive(m_read_m2_encoder_result)); } std::tuple Roboclaw::readM1M2Encoders() @@ -400,7 +404,7 @@ namespace rip messenger.send(m_device, m_read_m1m2_encoder, m_id, m_address); return messenger.receive(m_read_encoders_result); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_read_m1m2_encoder_result); } units::Distance Roboclaw::readEncoder(bool motor) @@ -431,7 +435,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read_m1_encoder_speed, m_id, m_address); - return std::get<0>(messenger.receive(m_read_encoder_speed_result)); + return std::get<0>(messenger.receive(m_read_m1_encoder_speed_result)); } int32_t Roboclaw::readM2EncoderSpeed() @@ -439,7 +443,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger messenger; messenger.send(m_device, m_read_m2_encoder_speed, m_id, m_address); - return std::get<0>(messenger.receive(m_read_encoder_speed_result)); + return std::get<0>(messenger.receive(m_read_m2_encoder_speed_result)); } std::tuple Roboclaw::readM1M2EncoderSpeed() @@ -448,7 +452,7 @@ namespace rip messenger.send(m_device, m_read_m1m2_encoder_speed, m_id, m_address); return messenger.receive(m_read_encoders_speed_result); + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType>(m_read_m1m2_encoder_speed_result); } std::array Roboclaw::readEncoderSpeeds() @@ -698,6 +702,10 @@ namespace rip bool Roboclaw::diagnostic() { + int32_t ticks; + units::Distance d; + units::Velocity v; + std::chrono::time_point start_time = std::chrono::system_clock::now(); misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics start"); //encoder reading diag @@ -820,7 +828,7 @@ namespace rip stop(); misc::Logger::getInstance()->debug("Roboclaw appendage diagnostics complete"); - return 0; + return 1; } } From eacb334f2081f731b4d69ce5cca45af66e021df6 Mon Sep 17 00:00:00 2001 From: Aiden Date: Sat, 31 Mar 2018 18:16:06 -0400 Subject: [PATCH 110/200] Added ticks/rev and wheel radius parameters to roboclaw.json --- appendages/json/roboclaw.json | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/appendages/json/roboclaw.json b/appendages/json/roboclaw.json index eb28aa1..0e33e41 100644 --- a/appendages/json/roboclaw.json +++ b/appendages/json/roboclaw.json @@ -2,7 +2,11 @@ "serial": "string", "baudrate": "int", "address": "int", + "ticks_per_rev" : "float", + "wheel_radius" : "float", "core": [ - "address" + "address", + "ticks_per_rev", + "wheel_radius" ] } From fea68b17fde6d45a1186d8e06b0d899080547476 Mon Sep 17 00:00:00 2001 From: Aiden Date: Mon, 2 Apr 2018 20:31:05 -0400 Subject: [PATCH 111/200] Added getDistAndVel --- appendages/include/appendages/roboclaw.hpp | 2 ++ appendages/src/roboclaw.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index 238dddd..f73c967 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -64,6 +64,8 @@ namespace rip void setDynamics(bool motor, const MotorDynamics& dynamics, bool respectBuffer=1); //Not implemented on arduino void setDynamics(const MotorDynamics& dynamics, bool respectBuffer=1); //Not implemented on arduino + + std::tuple getDistAndVel(bool motor); /** * Stop! ^0^ */ diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 3a20e60..a673cba 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -695,6 +695,11 @@ namespace rip m_address, Kp, Ki, Kd, qpps); } + std::tuple Roboclaw::getDistAndVel(bool motor) + { + return std::tuple(readEncoder(motor), readEncoderSpeed(motor)); + } + void Roboclaw::stop() { setM1M2Duty(0, 0); From 5e4408d3d84bce18c0ddc92455ec85111e332e82 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 3 Apr 2018 12:02:02 -0400 Subject: [PATCH 112/200] Fixes --- appendages/src/roboclaw.cpp | 12 +++++++++--- appendages/xml/roboclaw.xml | 2 +- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index a673cba..00277d3 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -62,6 +62,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, + cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::CharType>())), m_set_m1_speed_accel_dist(createCommand("kSetM1SpeedAccelDist", command_map, cmdmessenger::ArduinoCmdMessenger::makeArgumentString(m_device, m_set_m1m2_speed_accel_decel_dist, m_id, m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, flag); } @@ -805,15 +810,16 @@ namespace rip //speed accel decel dist drive diag misc::Logger::getInstance()->debug("Setting speed accel decel dist drive (5s)"); start_time = std::chrono::system_clock::now(); - + setM1M2SpeedAccelDecelDist(12000, 12000, 12000, 12000, 12000, 12000, 12000, 12000); + /* while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) { setM1SpeedAccelDecelDist(12000, 12000, 12000, 12000); setM2SpeedAccelDecelDist(12000, 12000, 12000, 12000); - setM1M2SpeedAccelDecelDist(12000, 12000, 12000, 12000, 12000, 12000, 12000, 12000); + misc::Logger::getInstance()->debug(fmt::format("Raw readings: 1: {} | 2: {}", readM1Encoder(), readM2Encoder())); misc::Logger::getInstance()->debug(fmt::format("Encoder velocity: M1 {} M2 {}", readM1EncoderSpeed(), readM2EncoderSpeed())); - } + }*/ stop(); //setDynamics diag misc::Logger::getInstance()->debug("Setting via setDynamics"); diff --git a/appendages/xml/roboclaw.xml b/appendages/xml/roboclaw.xml index 43ac912..7e452d0 100644 --- a/appendages/xml/roboclaw.xml +++ b/appendages/xml/roboclaw.xml @@ -207,7 +207,7 @@ if (!roboclaws[indexNum].SpeedAccelDeccelPositionM1M2(address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, flag)) { - cmdMessenger.sendBinCmd(kError, kSetM2SpeedAccelDecelDist); + cmdMessenger.sendBinCmd(kError, kSetM1M2SpeedAccelDecelDist); } From 85aa5380cb1b0ee584b4e48a7c07b629575564c2 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 3 Apr 2018 16:57:10 -0400 Subject: [PATCH 113/200] Add initialValue to servo init --- appendages/json/servo.json | 3 ++- appendages/xml/servo.xml | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/appendages/json/servo.json b/appendages/json/servo.json index 8bd4e95..d91013b 100644 --- a/appendages/json/servo.json +++ b/appendages/json/servo.json @@ -1,3 +1,4 @@ { - "pin": "int" + "pin": "int", + "initialValue": "int" } diff --git a/appendages/xml/servo.xml b/appendages/xml/servo.xml index a6f78d2..98c861d 100644 --- a/appendages/xml/servo.xml +++ b/appendages/xml/servo.xml @@ -8,9 +8,13 @@ + + + servos[$i$].attach($pin$); + servos[$i$].write($initialValue$); // Servo pin: $pin$ From 096dd2fbdbfc002cc2b0e983c0b8e85667487c24 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 3 Apr 2018 17:08:56 -0400 Subject: [PATCH 114/200] Dealing with decel func later --- appendages/src/roboclaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index 00277d3..acd43cd 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -369,7 +369,7 @@ namespace rip m_address, accel, speed, deccel, position, flag); } - void Roboclaw::setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, + /*void Roboclaw::setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2, uint8_t flag) { cmdmessenger::ArduinoCmdMessenger messenger; @@ -385,7 +385,7 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::UnsignedLongType, cmdmessenger::ArduinoCmdMessenger::CharType>(m_device, m_set_m1m2_speed_accel_decel_dist, m_id, m_address, accel1, speed1, deccel1, position1, accel2, speed2, deccel2, position2, flag); - } + }*/ int32_t Roboclaw::readM1Encoder() { From d41548c0fc6c2280759a54c1a9461b2990999b1d Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 3 Apr 2018 18:00:25 -0400 Subject: [PATCH 115/200] Fix to prior commit. Fully commenting out decel --- appendages/include/appendages/roboclaw.hpp | 4 ++-- appendages/src/roboclaw.cpp | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/appendages/include/appendages/roboclaw.hpp b/appendages/include/appendages/roboclaw.hpp index f73c967..24bb008 100644 --- a/appendages/include/appendages/roboclaw.hpp +++ b/appendages/include/appendages/roboclaw.hpp @@ -37,9 +37,9 @@ namespace rip void setM1SpeedAccelDecelDist(uint32_t accel, int32_t speed, uint32_t deccel, uint32_t position, uint8_t flag=1); void setM2SpeedAccelDecelDist(uint32_t accel, int32_t speed,uint32_t deccel,uint32_t position, uint8_t flag=1); - void setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, + /*void setM1M2SpeedAccelDecelDist(uint32_t accel1, int32_t speed1,uint32_t deccel1, uint32_t position1, uint32_t accel2, int32_t speed2, uint32_t deccel2, uint32_t position2, uint8_t flag=1); - + */ int32_t readM1Encoder(); int32_t readM2Encoder(); std::tuple readM1M2Encoders(); diff --git a/appendages/src/roboclaw.cpp b/appendages/src/roboclaw.cpp index acd43cd..d88d307 100644 --- a/appendages/src/roboclaw.cpp +++ b/appendages/src/roboclaw.cpp @@ -634,11 +634,13 @@ namespace rip } case MotorDynamics::DType::kSpeedAccelDecelDist: { + /* speed = static_cast((*dynamics.getSpeed() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); dist = static_cast((*dynamics.getDistance() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); accel = static_cast((*dynamics.getAcceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); decel = static_cast((*dynamics.getDeceleration() / m_wheel_radius / (units::pi * 2))() * m_ticks_per_rev); setM1M2SpeedAccelDecelDist(accel, speed, decel, dist, accel, speed, decel, dist, static_cast(respectBuffer)); + */ return; } default: @@ -810,7 +812,6 @@ namespace rip //speed accel decel dist drive diag misc::Logger::getInstance()->debug("Setting speed accel decel dist drive (5s)"); start_time = std::chrono::system_clock::now(); - setM1M2SpeedAccelDecelDist(12000, 12000, 12000, 12000, 12000, 12000, 12000, 12000); /* while(std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() < 5000) { From c8f3adf3e0059053adfa0b6f47b65b478389c779 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 3 Apr 2018 18:20:51 -0400 Subject: [PATCH 116/200] Make servos setup for .5s then detach --- appendages/include/appendages/servo.hpp | 3 +++ appendages/src/servo.cpp | 30 +++++++++++++++++++++++-- appendages/xml/servo.xml | 13 +++++++++++ 3 files changed, 44 insertions(+), 2 deletions(-) diff --git a/appendages/include/appendages/servo.hpp b/appendages/include/appendages/servo.hpp index 30ccd70..4e4f203 100644 --- a/appendages/include/appendages/servo.hpp +++ b/appendages/include/appendages/servo.hpp @@ -25,6 +25,8 @@ namespace rip */ void write(int value); + void attach(bool state); + /** * Stop */ @@ -55,6 +57,7 @@ namespace rip Servo(const nlohmann::json& config, const std::map& command_map, std::shared_ptr device); std::shared_ptr m_write; + std::shared_ptr m_attach; }; } } diff --git a/appendages/src/servo.cpp b/appendages/src/servo.cpp index b1a0676..0990b14 100644 --- a/appendages/src/servo.cpp +++ b/appendages/src/servo.cpp @@ -21,6 +21,15 @@ namespace rip cmdmessenger::ArduinoCmdMessenger::IntegerType>() ) ) + , m_attach( + createCommand( + "kAttachServo", + command_map, + cmdmessenger::ArduinoCmdMessenger::makeArgumentString< + cmdmessenger::ArduinoCmdMessenger::IntegerType, + cmdmessenger::ArduinoCmdMessenger::BooleanType>() + ) + ) { } @@ -32,19 +41,36 @@ namespace rip void Servo::stop() { + attach(false); + } + void Servo::attach(bool state) + { + cmdmessenger::ArduinoCmdMessenger messenger; + messenger.send(m_device, m_attach, m_id, state); } bool Servo::diagnostic() { int new_val = 0; + std::cout << " === DIAG CONTROLS: === " << '\n'; + std::cout << " '-1': Quit." << '\n'; + std::cout << " '-2': Detach servo." << '\n'; + std::cout << " '0-180': Set servo position." << '\n'; while (new_val != -1) { // write(0); std::cout << " >>> Please enter a servo value (int) to write (-1 quits): "; std::cin >> new_val; + std::cout << " >>> Working...\n"; if (new_val == -1) break; - std::cout << " >>> Writing...\n"; - write(new_val); + else if (new_val == -2) + { + attach(false); + } + else + { + write(new_val); + } } return true; } diff --git a/appendages/xml/servo.xml b/appendages/xml/servo.xml index 98c861d..5ae10c7 100644 --- a/appendages/xml/servo.xml +++ b/appendages/xml/servo.xml @@ -15,6 +15,8 @@ servos[$i$].attach($pin$); servos[$i$].write($initialValue$); + delay(500); + servos[$i$].detach(); // Servo pin: $pin$ @@ -29,5 +31,16 @@ servos[indexNum].write(value); + + + + if (state) { + servos[indexNum].attach(servo_pins[indexNum]); + } + else { + servos[indexNum].detach(); + } + + From 4d51dde166778cf114ca5c64f08addf96ceaf514 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 3 Apr 2018 19:16:51 -0400 Subject: [PATCH 117/200] Two roboclaw appendage compiles --- .../two_roboclaw_appendage_drivetrain.hpp | 102 ++++++++ .../src/two_roboclaw_appendage_drivetrain.cpp | 223 ++++++++++++++++++ 2 files changed, 325 insertions(+) create mode 100644 core/navigation/drivetrains/include/drivetrains/two_roboclaw_appendage_drivetrain.hpp create mode 100644 core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp diff --git a/core/navigation/drivetrains/include/drivetrains/two_roboclaw_appendage_drivetrain.hpp b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_appendage_drivetrain.hpp new file mode 100644 index 0000000..91969b2 --- /dev/null +++ b/core/navigation/drivetrains/include/drivetrains/two_roboclaw_appendage_drivetrain.hpp @@ -0,0 +1,102 @@ +#ifndef TWO_ROBOCLAW_DRIVE_TRAIN_HPP +#define TWO_ROBOCLAW_DRIVE_TRAIN_HPP + +#include +#include +#include +#include +#include "drivetrain.hpp" +#include + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + /** + * Abstract base class for the drive train + */ + + class TwoRoboclawAppendageDrivetrain : public Drivetrain + { + using Roboclaw = rip::appendages::Roboclaw; + using NavX = navx::NavX; + using MotorDynamics = motorcontrollers::MotorDynamics; + public: + TwoRoboclawAppendageDrivetrain(const std::string& name, std::shared_ptr left, + std::shared_ptr right, std::shared_ptr navx = nullptr); + + ~TwoRoboclawAppendageDrivetrain(); + + /** + * Drive all the motors + * @param power [-1.0, 1.0] + */ + virtual void drive(double power) override; + + /** + * Drive left and right separately + * @param left [-1.0, 1.0] + * @param right [-1.0, 1.0] + */ + virtual void drive(double left, double right) override; + + /** + * Drive all the motors + * + * all range from [-1.0, 1.0] + */ + virtual void drive(double front_left, double front_right, double back_left, + double back_rightk) override; + + /** + * Single command to all motors + */ + virtual void drive(const MotorDynamics& command) override; + + /** + * Command left and right sides separately + */ + virtual void drive(const MotorDynamics& left, const MotorDynamics& right) override; + + /** + * Command four wheels separately + */ + virtual void drive(const MotorDynamics& front_left, const MotorDynamics& front_right, + const MotorDynamics& back_left, const MotorDynamics& back_right) override; + + /** + * Reads encoders for every motor you tell it to read, reports back in respective + * order + */ + virtual std::vector readEncoders(const std::vector& motors) override; + /** + * reads the encoder for one motor + */ + virtual units::Distance readEncoder(const Motor& motor) override; + /** + * Reads encoder velocity for every motor you tell it to read, reports back in respective + * order + * @param motors list of motors to read + */ + virtual std::vector readEncoderVelocities(const std::vector& motors) override; + /** + * reads the encoder for one motor + */ + virtual units::Velocity readEncoderVelocity(const Motor& motor) override; + + virtual void stop() override; + + virtual bool diagnostic() override; + private: + + std::shared_ptr m_left; + std::shared_ptr m_right; + std::shared_ptr m_navx; + }; + } + } +} + +#endif // DRIVE_TRAIN_HPP diff --git a/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp new file mode 100644 index 0000000..8d6f045 --- /dev/null +++ b/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp @@ -0,0 +1,223 @@ +#include "drivetrains/two_roboclaw_appendage_drivetrain.hpp" +#include +#include +#include +#include + +namespace rip +{ + namespace navigation + { + namespace drivetrains + { + TwoRoboclawAppendageDrivetrain::TwoRoboclawAppendageDrivetrain(const std::string& name, + std::shared_ptr left, + std::shared_ptr right, + std::shared_ptr navx) + : Drivetrain(name) + , m_left(left) + , m_right(right) + , m_navx(navx) + {} + + TwoRoboclawAppendageDrivetrain::~TwoRoboclawAppendageDrivetrain() + { + stop(); + } + + void TwoRoboclawAppendageDrivetrain::drive(double power) + { + if (std::abs(power) > 1) + { + throw OutOfRangeException("out of range"); + } + m_left->setM1M2Duty(static_cast(32767 * power), static_cast(32767 * power)); + m_right->setM1M2Duty(static_cast(32767 * power), static_cast(32767 * power)); + } + + void TwoRoboclawAppendageDrivetrain::drive(double left, double right) + { + if (std::abs(left) > 1 || std::abs(right) > 1) + { + throw OutOfRangeException("out of range"); + } + m_left->setM1M2Duty(static_cast(32767 * left), static_cast(32767 * left)); + m_right->setM1M2Duty(static_cast(32767 * right), static_cast(32767 * right)); + } + + void TwoRoboclawAppendageDrivetrain::drive(double front_left, double front_right, double back_left, + double back_right) + { + if (std::abs(front_left) > 1 || std::abs(front_right) > 1 || std::abs(back_left) > 1 || + std::abs(back_right) > 1) + { + throw OutOfRangeException("out of range"); + } + + m_left->setM1Duty(static_cast(32767 * front_left)); + m_right->setM1Duty(static_cast(32767 * front_right)); + + m_left->setM2Duty(static_cast(32767 * back_left)); + m_right->setM2Duty(static_cast(32767 * back_right)); + } + + void TwoRoboclawAppendageDrivetrain::drive(const MotorDynamics& command) + { + m_left->setDynamics(command); + m_right->setDynamics(command); + } + + void TwoRoboclawAppendageDrivetrain::drive(const MotorDynamics& left, const MotorDynamics& right) + { + m_left->setDynamics(left); + m_right->setDynamics(right); + } + + void TwoRoboclawAppendageDrivetrain::drive(const MotorDynamics& front_left, const MotorDynamics& front_right, + const MotorDynamics& back_left, const MotorDynamics& back_right) + { + m_left->setDynamics(0, front_left); + m_right->setDynamics(0, front_right); + + m_left->setDynamics(1, back_left); + m_right->setDynamics(1, back_right); + + } + + std::vector TwoRoboclawAppendageDrivetrain::readEncoders(const std::vector& motors) + { + std::vector data; + for(uint i=0; ireadEncoder(0)); + break; + } + case Motor::kFrontRight: + { + data.push_back(m_right->readEncoder(0)); + break; + } + case Motor::kBackLeft: + { + data.push_back(m_left->readEncoder(1)); + break; + } + case Motor::kBackRight: + { + data.push_back(m_right->readEncoder(1)); + break; + } + default: + { + throw InvalidMotorException(fmt::format("Invalid motor, parameter {}", i+1)); + } + } + } + return data; + } + + units::Distance TwoRoboclawAppendageDrivetrain::readEncoder(const Motor& motor) + { + switch(motor) + { + case Motor::kFrontLeft: + { + return m_left->readEncoder(0); + } + case Motor::kFrontRight: + { + return m_right->readEncoder(0); + } + case Motor::kBackLeft: + { + return m_left->readEncoder(1); + } + case Motor::kBackRight: + { + return m_right->readEncoder(1); + } + default: + { + throw InvalidMotorException("Invalid motor"); + } + } + } + + std::vector TwoRoboclawAppendageDrivetrain::readEncoderVelocities( + const std::vector& motors) + { + std::vector data; + for(uint i=0; ireadEncoderSpeed(0)); + } + case Motor::kFrontRight: + { + data.push_back(m_right->readEncoderSpeed(0)); + } + case Motor::kBackLeft: + { + data.push_back(m_left->readEncoderSpeed(1)); + } + case Motor::kBackRight: + { + data.push_back(m_right->readEncoderSpeed(1)); + } + default: + { + throw InvalidMotorException(fmt::format("Invalid motor, parameter {}", i+1)); + } + } + } + return data; + } + + units::Velocity TwoRoboclawAppendageDrivetrain::readEncoderVelocity(const Motor& motor) + { + switch(motor) + { + case Motor::kFrontLeft: + { + return m_left->readEncoderSpeed(0); + } + case Motor::kFrontRight: + { + return m_right->readEncoderSpeed(0); + } + case Motor::kBackLeft: + { + return m_left->readEncoderSpeed(1); + } + case Motor::kBackRight: + { + return m_right->readEncoderSpeed(1); + } + default: + { + throw InvalidMotorException("Invalid motor"); + } + } + } + + void TwoRoboclawAppendageDrivetrain::stop() + { + m_left->stop(); + m_right->stop(); + } + + bool TwoRoboclawAppendageDrivetrain::diagnostic() + { + // todo + return 0; + } + } + }//subsystem +}//rip From ceb19672d3c45572bc529ea563410e65dde9c1d4 Mon Sep 17 00:00:00 2001 From: Aiden Date: Tue, 3 Apr 2018 19:25:00 -0400 Subject: [PATCH 118/200] namespace fix --- .../drivetrains/src/two_roboclaw_appendage_drivetrain.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp b/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp index 8d6f045..cadf738 100644 --- a/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp +++ b/core/navigation/drivetrains/src/two_roboclaw_appendage_drivetrain.cpp @@ -11,8 +11,8 @@ namespace rip namespace drivetrains { TwoRoboclawAppendageDrivetrain::TwoRoboclawAppendageDrivetrain(const std::string& name, - std::shared_ptr left, - std::shared_ptr right, + std::shared_ptr left, + std::shared_ptr right, std::shared_ptr navx) : Drivetrain(name) , m_left(left) From a5bd132b590d3c363043b9e8be89f0e481935070 Mon Sep 17 00:00:00 2001 From: Ben Klein Date: Tue, 3 Apr 2018 19:25:18 -0400 Subject: [PATCH 119/200] Adding docs --- build-linux.sh | 36 +++++++++++++++++++++++++++++++----- go-docker.sh | 39 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+), 5 deletions(-) diff --git a/build-linux.sh b/build-linux.sh index 1f439ce..4000d3e 100755 --- a/build-linux.sh +++ b/build-linux.sh @@ -9,11 +9,6 @@ fi while [[ "$1" != "" ]]; do case "$1" in - "--help"|"-h") - echo "$0