From 108f4ef4a1155c391eabc4b964f829e5b8625554 Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Mon, 11 Apr 2022 16:33:31 -0700 Subject: [PATCH 01/29] Some lint --- .../src/ArmControlSystem.cpp | 26 +++++++++++++------ 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 68f29ed6..fe315ac5 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -15,6 +15,8 @@ #include #include #include +#include +#include #include #include "SimpleJoint.hpp" #include "DifferentialJoint.hpp" @@ -22,17 +24,24 @@ using XmlRpc::XmlRpcValue; +/** + * @brief Refresh rate of ros::Rate + */ +const float CLOCK_RATE = 2; + /** * @brief Defines space for all ArmMotor references */ const int numMotors = 6; -ArmMotor *motors[numMotors]; +std::array, numMotors> motors; +// ArmMotor *motors[numMotors]; /** * @brief Defines space for all Joint references */ const int numJoints = 5; -AbstractJoint *joints[numJoints]; +std::array, numJoints> joints; +// AbstractJoint *joints[numJoints]; /** * @brief The Joint State Publisher for MoveIt */ @@ -115,7 +124,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server // Track whether or not the current position is done bool hasPositionFinished = false; // Keep max loop rate at 50 Hz - ros::Rate loop(200); + ros::Rate loop(CLOCK_RATE); // While the current position is not complete yet... while(!hasPositionFinished){ @@ -132,6 +141,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server AbstractJoint *joint; // For each joint specified in the currTargetPosition... + // TODO: revise later for(int j = 0; j < sizeof(joints); j += sizeof(joints[jointIndex-1])){ joint = joints[jointIndex]; @@ -178,7 +188,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server //When all positions have been reached, set the current task as succeeded for(int i = 0; i < numMotors; i++){ - motors[i]->setPower(0.f); + motors.at(i)->setPower(0.0F); } as->setSucceeded(); @@ -193,8 +203,8 @@ void publish(const ros::TimerEvent &event){ sensor_msgs::JointState js_msg; for (int i = 0; i < numMotors; i++){ - names.push_back(motors[i]->getMotorName()); - positions.push_back(motors[i]->getRads()); + names.push_back(motors.at(i)->getMotorName()); + positions.push_back(motors.at(i)->getRads()); } positions[4] = positions[5] + positions[4] / 2; @@ -225,7 +235,7 @@ int main(int argc, char** argv) pn.getParam("encoder_parameters", encParams); // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node - motors[0] = new ArmMotor("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); + motors.at(0) = new ArmMotor("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); motors[1] = new ArmMotor("forearm_roll", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); motors[2] = new ArmMotor("shoulder", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); motors[3] = new ArmMotor("turntable", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n); @@ -234,7 +244,7 @@ int main(int argc, char** argv) std::cout << "init motors" << std::endl; // Initialize all Joints - joints[0] = new SimpleJoint(motors[0], &n); + joints.at(0) = new SimpleJoint(motors[0], &n); joints[1] = new SimpleJoint(motors[1], &n); joints[2] = new SimpleJoint(motors[2], &n); joints[3] = new SimpleJoint(motors[3], &n); From 54c080718bbce1c4c8820d086ebe86973283fc45 Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Mon, 11 Apr 2022 17:26:00 -0700 Subject: [PATCH 02/29] Finished de-linting ArmMotor --- src/wr_control_drive_arm/src/ArmMotor.cpp | 12 +++++++----- src/wr_control_drive_arm/src/ArmMotor.hpp | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index e9fc2267..39ec23c2 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -9,7 +9,9 @@ /// Allow for referencing the UInt32 message type easier typedef std_msgs::UInt32::ConstPtr Std_UInt32; typedef std_msgs::Float64::ConstPtr Std_Float64; -#define Std_Bool std_msgs::Bool::ConstPtr& +typedef std_msgs::Bool::ConstPtr Std_Bool; + +constexpr float STALL_THRESHOLD_TIME = 0.5; double ArmMotor::corrMod(double i, double j){ // Stem i%j by j, which in modular arithmetic is the same as adding 0. @@ -44,8 +46,8 @@ void ArmMotor::redirectPowerOutput(const Std_Float64 &msg){ this->setPower(msg->data); } -void ArmMotor::storeStallStatus(const Std_Bool msg) { - this->isStall = msg->data; +void ArmMotor::storeStallStatus(const Std_Bool &msg) { + this->isStall = static_cast(msg->data); } /// controllerID is constrained between [0,3] @@ -127,8 +129,8 @@ void ArmMotor::setPower(float power){ bool ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ // Checks for stall if (this->isStall) { - if ((ros::Time::now() - begin).toSec() >= 0.5) { - this->setPower(0.0f); + if ((ros::Time::now() - begin).toSec() >= STALL_THRESHOLD_TIME) { + this->setPower(0.0F); this->currState = MotorState::STOP; return false; } diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 0124a9b1..7b1626af 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -62,7 +62,7 @@ class ArmMotor{ /// The most recent power message sent std_msgs::Int16 powerMsg; /// If the motor is stalling or not - volatile bool isStall; + volatile bool isStall = false; /// The ROS Subscriber that reads stall status data ros::Subscriber stallRead; /// The time when the motor began stalling From 6de943b7e723ba0eb6562c1e72a109886cca45dd Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Mon, 11 Apr 2022 17:44:52 -0700 Subject: [PATCH 03/29] Incomplete de-linting on ArmControlSystem --- .../src/ArmControlSystem.cpp | 47 +++++++++++-------- 1 file changed, 28 insertions(+), 19 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index fe315ac5..032275b8 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -27,20 +27,30 @@ using XmlRpc::XmlRpcValue; /** * @brief Refresh rate of ros::Rate */ -const float CLOCK_RATE = 2; +constexpr float CLOCK_RATE = 2; + +/** + * @brief Nessage cache size of publisher + */ +constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1000; + +/** + * @brief Period between timer callback + */ +constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0; /** * @brief Defines space for all ArmMotor references */ -const int numMotors = 6; -std::array, numMotors> motors; +constexpr int NUM_MOTORS = 6; +std::array, NUM_MOTORS> motors; // ArmMotor *motors[numMotors]; /** * @brief Defines space for all Joint references */ -const int numJoints = 5; -std::array, numJoints> joints; +constexpr int NUM_JOINTS = 5; +std::array, NUM_JOINTS> joints; // AbstractJoint *joints[numJoints]; /** * @brief The Joint State Publisher for MoveIt @@ -110,7 +120,7 @@ bool configJointSetpoint(AbstractJoint* joint, int degreeIndex, double target, f void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { if (!IKEnabled) { as->setAborted(); - ROS_WARN("IK is disabled"); + ROS_WARN("IK is disabled"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) return; } @@ -164,14 +174,11 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server IKEnabled = false; std_srvs::Trigger srv; if (enableServiceClient.call(srv)) { - ROS_WARN("%s", (std::string{"PLACEHOLDER_NAME: "} + srv.response.message).data()); + ROS_WARN("%s", (std::string{"PLACEHOLDER_NAME: "} + srv.response.message).data()); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) } else { - ROS_WARN("Error: failed to call service PLACEHOLDER_NAME"); + ROS_WARN("Error: failed to call service PLACEHOLDER_NAME"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) } return; - // TODO: hand control back to driver - // standard service empty stdsrvs empty - // go into spin loop } jointIndex++; } @@ -187,8 +194,8 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server //When all positions have been reached, set the current task as succeeded - for(int i = 0; i < numMotors; i++){ - motors.at(i)->setPower(0.0F); + for(auto &motor : motors){ + motor->setPower(0.0F); } as->setSucceeded(); @@ -202,9 +209,9 @@ void publish(const ros::TimerEvent &event){ std::vector positions; sensor_msgs::JointState js_msg; - for (int i = 0; i < numMotors; i++){ - names.push_back(motors.at(i)->getMotorName()); - positions.push_back(motors.at(i)->getRads()); + for (auto &motor : motors){ + names.push_back(motor->getMotorName()); + positions.push_back(motor->getRads()); } positions[4] = positions[5] + positions[4] / 2; @@ -235,6 +242,7 @@ int main(int argc, char** argv) pn.getParam("encoder_parameters", encParams); // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node + // TODO: fix motors.at(0) = new ArmMotor("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); motors[1] = new ArmMotor("forearm_roll", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); motors[2] = new ArmMotor("shoulder", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); @@ -244,6 +252,7 @@ int main(int argc, char** argv) std::cout << "init motors" << std::endl; // Initialize all Joints + // TODO: fix joints.at(0) = new SimpleJoint(motors[0], &n); joints[1] = new SimpleJoint(motors[1], &n); joints[2] = new SimpleJoint(motors[2], &n); @@ -256,7 +265,7 @@ int main(int argc, char** argv) joints[5] = temp; // Initialize the Joint State Data Publisher - jointStatePublisher = n.advertise("/control/arm_joint_states", 1000); + jointStatePublisher = n.advertise("/control/arm_joint_states", MESSAGE_CACHE_SIZE); // Initialize the Action Server Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); @@ -264,14 +273,14 @@ int main(int argc, char** argv) server.start(); std::cout << "server started" << std::endl; - ros::Timer timer = n.createTimer(ros::Duration(1.0 / 50.0), publish); + ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publish); enableServiceServer = n.advertiseService("start_IK", static_cast>( [](std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)->bool{ IKEnabled = true; res.message = "Arm IK Enabled"; - res.success = true; + res.success = static_cast(true); return true; } )); From f90bf52f11bc8b7017f3607c96d543e1c9e59945 Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Mon, 11 Apr 2022 17:46:09 -0700 Subject: [PATCH 04/29] mispelling --- src/wr_control_drive_arm/src/ArmMotor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 39ec23c2..91188e5f 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -53,7 +53,7 @@ void ArmMotor::storeStallStatus(const Std_Bool &msg) { /// controllerID is constrained between [0,3] /// motorID is constrained between [0,1] ArmMotor::ArmMotor( - const std::string &motorName, + const std::string &motor_name, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, @@ -62,7 +62,7 @@ ArmMotor::ArmMotor( ) : COUNTS_PER_ROTATION{countsPerRotation}, ENCODER_BOUNDS{0, std::abs(countsPerRotation)}, ENCODER_OFFSET{offset}, - motorName{motorName}, + motorName{motor_name}, controllerID{controllerID}, motorID{motorID}, currState{MotorState::STOP}, From 88fc9a6db5c25de782f06dc980c0e9fc073b4bb2 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Mon, 11 Apr 2022 19:47:12 -0500 Subject: [PATCH 05/29] First round de-linting of AbstractJoint --- .../src/AbstractJoint.cpp | 28 +++++------ .../src/AbstractJoint.hpp | 48 ++++++++----------- 2 files changed, 32 insertions(+), 44 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp index 2c42679b..42714e5f 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.cpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.cpp @@ -6,35 +6,29 @@ */ #include "AbstractJoint.hpp" -AbstractJoint::AbstractJoint(ros::NodeHandle* n, int numMotors){ - this->n = n; - this->numMotors = numMotors; - - jointPositions.reserve(numMotors); - jointVelocites.reserve(numMotors); - +AbstractJoint::AbstractJoint(ros::NodeHandle &n, int numMotors){ + this->motors.resize(numMotors); } -int AbstractJoint::getDegreesOfFreedom(){ - return this->numMotors; +unsigned int AbstractJoint::getDegreesOfFreedom() const{ + return this->motors.size(); } -ArmMotor* AbstractJoint::getMotor(int motorIndex){ - return this->motors[motorIndex]; +const std::unique_ptr& AbstractJoint::getMotor(int motorIndex) const{ + return this->motors.at(motorIndex).motor; } void AbstractJoint::configSetpoint(int degreeIndex, double position, double velocity){ - this->jointPositions[degreeIndex] = position; - this->jointVelocites[degreeIndex] = velocity; + this->motors.at(degreeIndex).position = position; + this->motors.at(degreeIndex).velocity = velocity; } bool AbstractJoint::exectute(){ - vector motorPositions; - this->getMotorPositions(jointPositions, motorPositions); - for(int i = 0; i < this->numMotors; i++){ - if (!this->motors[i]->runToTarget(motorPositions[i], 0)) return false; + for(auto &motorHandle : motors){ + if (!motorHandle.motor->runToTarget(motorHandle.position, 0)) + return false; } return true; } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index 978345e9..2737ce07 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -5,39 +5,33 @@ using std::vector; class AbstractJoint { +public: + struct MotorHandler{ + const std::unique_ptr motor; + double position; + double velocity; + std::string jointTopicName; + std::string motorTopicName; + bool newVelocities; + }; + AbstractJoint(ros::NodeHandle &n, int numMotors); - protected: - ros::NodeHandle* n; - unsigned int numMotors; - vector motors; + // never used, need to be defined for compiler v-table + virtual void getMotorPositions(vector &jointPositions, vector &target) = 0; + virtual void getMotorVelocities(vector &joinVelocities, vector &target) = 0; + virtual void getJointPositions(vector &motorPositions, vector &target) = 0; - vector jointPositions; - vector jointVelocites; + unsigned int getDegreesOfFreedom() const; + + const std::unique_ptr& getMotor(int motorIndex) const; - vector jointTopicsNames; - vector motorTopicsNames; - vector newVelocitiesVector; + void configSetpoint(int degreeIndex, double position, double velocity); - public: + bool exectute(); - AbstractJoint(ros::NodeHandle* n, int numMotors); - ~AbstractJoint(){}; - - // never used, need to be defined for compiler v-table - virtual void getMotorPositions(vector &jointPositions, vector &target) = 0; - virtual void getMotorVelocities(vector &joinVelocities, vector &target) = 0; - virtual void getJointPositions(vector &motorPositions, vector &target) = 0; - - int getDegreesOfFreedom(); - - ArmMotor* getMotor(int motorIndex); - - void configSetpoint(int degreeIndex, double position, double velocity); - - bool exectute(); - - // virtual void configVelocityHandshake(std::string, std::string) = 0; +protected: + vector motors; }; #endif \ No newline at end of file From 3904d85213e319b5243fc6af9b35daf7087161f5 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 12 Apr 2022 12:06:37 -0500 Subject: [PATCH 06/29] Redo-ing some lint with an additional `clang-tidy` options --- .../src/AbstractJoint.cpp | 6 +-- .../src/AbstractJoint.hpp | 10 ++-- src/wr_control_drive_arm/src/ArmMotor.cpp | 50 +++++++++---------- src/wr_control_drive_arm/src/ArmMotor.hpp | 34 +++++++------ .../src/DifferentialJoint.cpp | 6 +-- .../src/DifferentialJoint.hpp | 2 +- 6 files changed, 55 insertions(+), 53 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp index 42714e5f..dab9a278 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.cpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.cpp @@ -10,11 +10,11 @@ AbstractJoint::AbstractJoint(ros::NodeHandle &n, int numMotors){ this->motors.resize(numMotors); } -unsigned int AbstractJoint::getDegreesOfFreedom() const{ +auto AbstractJoint::getDegreesOfFreedom() const -> unsigned int{ return this->motors.size(); } -const std::unique_ptr& AbstractJoint::getMotor(int motorIndex) const{ +auto AbstractJoint::getMotor(int motorIndex) const -> const std::unique_ptr&{ return this->motors.at(motorIndex).motor; } @@ -25,7 +25,7 @@ void AbstractJoint::configSetpoint(int degreeIndex, double position, double velo } -bool AbstractJoint::exectute(){ +auto AbstractJoint::exectute() -> bool{ for(auto &motorHandle : motors){ if (!motorHandle.motor->runToTarget(motorHandle.position, 0)) return false; diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index 2737ce07..c187eff4 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -7,12 +7,12 @@ using std::vector; class AbstractJoint { public: struct MotorHandler{ - const std::unique_ptr motor; + std::unique_ptr motor; double position; double velocity; std::string jointTopicName; std::string motorTopicName; - bool newVelocities; + bool newVelocity; }; AbstractJoint(ros::NodeHandle &n, int numMotors); @@ -22,13 +22,13 @@ class AbstractJoint { virtual void getMotorVelocities(vector &joinVelocities, vector &target) = 0; virtual void getJointPositions(vector &motorPositions, vector &target) = 0; - unsigned int getDegreesOfFreedom() const; + auto getDegreesOfFreedom() const -> unsigned int; - const std::unique_ptr& getMotor(int motorIndex) const; + auto getMotor(int motorIndex) const -> const std::unique_ptr&; void configSetpoint(int degreeIndex, double position, double velocity); - bool exectute(); + auto exectute() -> bool; protected: vector motors; diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index 91188e5f..d081694d 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -5,30 +5,30 @@ * @date 2021-04-05 */ #include "ArmMotor.hpp" +#include +#include /// Allow for referencing the UInt32 message type easier -typedef std_msgs::UInt32::ConstPtr Std_UInt32; -typedef std_msgs::Float64::ConstPtr Std_Float64; -typedef std_msgs::Bool::ConstPtr Std_Bool; +using Std_UInt32 = std_msgs::UInt32::ConstPtr; +using Std_Float64 = std_msgs::Float64::ConstPtr; +using Std_Bool = std_msgs::Bool::ConstPtr; -constexpr float STALL_THRESHOLD_TIME = 0.5; - -double ArmMotor::corrMod(double i, double j){ +auto ArmMotor::corrMod(double i, double j) -> double{ // Stem i%j by j, which in modular arithmetic is the same as adding 0. return std::fmod(std::fmod(std::abs(j)*i/j,std::abs(j))+j,std::abs(j)); } /// Currently consistent with the rad->enc equation as specified here. -uint32_t ArmMotor::radToEnc(double rads) const { +auto ArmMotor::radToEnc(double rads) const -> uint32_t{ return this->COUNTS_PER_ROTATION * ArmMotor::corrMod(rads, 2 * M_PI)/(2 * M_PI) + this->ENCODER_OFFSET; } -double ArmMotor::encToRad(uint32_t enc) const { +auto ArmMotor::encToRad(uint32_t enc) const -> double{ return ArmMotor::corrMod(static_cast(enc - this->ENCODER_OFFSET) / static_cast(this->COUNTS_PER_ROTATION) * 2 * M_PI + M_PI, 2 * M_PI) - M_PI; } /// Currently consistent with the enc->rad equation as specified here. -double ArmMotor::getRads() const{ +auto ArmMotor::getRads() const -> double{ return ArmMotor::encToRad(this->getEncoderCounts()); } @@ -53,7 +53,7 @@ void ArmMotor::storeStallStatus(const Std_Bool &msg) { /// controllerID is constrained between [0,3] /// motorID is constrained between [0,1] ArmMotor::ArmMotor( - const std::string &motor_name, + std::string motor_name, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, @@ -62,18 +62,18 @@ ArmMotor::ArmMotor( ) : COUNTS_PER_ROTATION{countsPerRotation}, ENCODER_BOUNDS{0, std::abs(countsPerRotation)}, ENCODER_OFFSET{offset}, - motorName{motor_name}, + motorName{std::move(motor_name)}, controllerID{controllerID}, motorID{motorID}, currState{MotorState::STOP}, encoderVal{static_cast(offset)} { // Check validity of WRoboclaw and motor IDs - if(controllerID > 3) throw ((std::string)"Controller ID ") + std::to_string(controllerID) + "is only valid on [0,3]"; - if(motorID > 1) throw ((std::string)"Motor ID ") + std::to_string(motorID) + "is only valid on [0,1]"; + if(controllerID > 3) throw std::invalid_argument{std::string{"Controller ID "} + std::to_string(controllerID) + "is only valid on [0,3]"}; + if(motorID > 1) throw std::invalid_argument{std::string{"Motor ID "} + std::to_string(motorID) + " is only valid on [0,1]"}; // Create the topic string prefix for WRoboclaw controllers - std::string tpString = ((std::string)"/hsi/roboclaw/aux") + std::to_string(controllerID); + std::string tpString = std::string{"/hsi/roboclaw/aux"} + std::to_string(controllerID); std::string controlString = "/control/arm/" + std::to_string(controllerID) + std::to_string(motorID); // Create the appropriate encoder-reading and speed-publishing subscribers and advertisers, respectfully @@ -85,7 +85,7 @@ ArmMotor::ArmMotor( this->stallRead = n.subscribe(tpString + "/curr/over_lim/" + (motorID == 0 ? "left" : "right"), ArmMotor::MESSAGE_CACHE_SIZE, &ArmMotor::storeStallStatus, this); } -uint32_t ArmMotor::getEncoderCounts() const{ +auto ArmMotor::getEncoderCounts() const -> uint32_t{ return this->encoderVal; } @@ -94,7 +94,7 @@ void ArmMotor::runToTarget(uint32_t targetCounts, float power){ this->runToTarget(targetCounts, power, false); } -bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const { +auto ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const -> bool{ // Compute the upper and lower bounds in the finite encoder space uint32_t lBound = ArmMotor::corrMod(static_cast(targetCounts - tolerance), static_cast(ArmMotor::ENCODER_BOUNDS[1])); uint32_t uBound = ArmMotor::corrMod(static_cast(targetCounts + tolerance), static_cast(ArmMotor::ENCODER_BOUNDS[1])); @@ -106,18 +106,18 @@ bool ArmMotor::hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const } /// Current tolerance is ±0.1 degree w.r.t. the current number of counts per rotation -bool ArmMotor::hasReachedTarget(uint32_t targetCounts) const { +auto ArmMotor::hasReachedTarget(uint32_t targetCounts) const -> bool{ return ArmMotor::hasReachedTarget(targetCounts, ArmMotor::TOLERANCE_RATIO * static_cast(std::abs(this->COUNTS_PER_ROTATION))); } -MotorState ArmMotor::getMotorState() const { +auto ArmMotor::getMotorState() const -> MotorState{ return this->currState; } /// This method auto-publishes the speed command to the WRoboclaws void ArmMotor::setPower(float power){ // Check the bounds of the parameter - if(abs(power) > 1) throw ((std::string)"Power ") + std::to_string(power) + " is not on the interval [-1, 1]"; + if(abs(power) > 1) throw std::invalid_argument{std::string{"Power "} + std::to_string(power) + " is not on the interval [-1, 1]"}; // Set up and send the speed message this->powerMsg.data = power * INT16_MAX; @@ -126,7 +126,7 @@ void ArmMotor::setPower(float power){ this->currState = power == 0.F ? MotorState::STOP : MotorState::MOVING; } -bool ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ +auto ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block) -> bool{ // Checks for stall if (this->isStall) { if ((ros::Time::now() - begin).toSec() >= STALL_THRESHOLD_TIME) { @@ -165,24 +165,24 @@ bool ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block){ return true; } -bool ArmMotor::runToTarget(double rads, float power){ +auto ArmMotor::runToTarget(double rads, float power) -> bool{ std::cout << "run to target: " << rads << ":" << this->radToEnc(rads) << std::endl; return runToTarget(this->radToEnc(rads), power, false); } -std::string ArmMotor::getMotorName() const { +auto ArmMotor::getMotorName() const -> std::string{ return this->motorName; } -unsigned int ArmMotor::getMotorID() const { +auto ArmMotor::getMotorID() const -> unsigned int{ return this->motorID; } -unsigned int ArmMotor::getControllerID() const { +auto ArmMotor::getControllerID() const -> unsigned int{ return this->controllerID; } -float ArmMotor::getPower() const { +auto ArmMotor::getPower() const -> float{ return ((float)this->powerMsg.data)/INT16_MAX; } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 7b1626af..0962158c 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -10,7 +10,7 @@ #include "std_msgs/Int16.h" #include "std_msgs/Float64.h" #include "std_msgs/Bool.h" -#include "math.h" +#include #include /** @@ -29,6 +29,8 @@ enum class MotorState{ */ class ArmMotor{ private: + /// Time tolerance of over-current detection before a stall fault is triggered + static constexpr float STALL_THRESHOLD_TIME = 0.5; /// Tolerance ratio w.r.t the counts per rotation for encoder run-to-position motion static constexpr double TOLERANCE_RATIO = 0.1/360; /// Size of ROS Topic message caches @@ -74,7 +76,7 @@ class ArmMotor{ * @param rad The input number of radians * @return uint32_t The corresponding encoder count bounded by ENCODER_BOUNDS */ - uint32_t radToEnc(double rad) const; + auto radToEnc(double rad) const -> uint32_t; /** * @brief A static conversion from encoder counts to radians @@ -82,7 +84,7 @@ class ArmMotor{ * @param enc The input number of encoder counts * @return double The corresponding radian measure */ - double encToRad(uint32_t enc) const; + auto encToRad(uint32_t enc) const -> double; /** * @brief Subscriber callback for encRead, captures the encoder value of the current motor @@ -112,7 +114,7 @@ class ArmMotor{ * @param j The divisor of the modulus * @return double The Euclidean-correct remainder bounded on [0, j) */ - static double corrMod(double i, double j); + static auto corrMod(double i, double j) -> double; public: /** @@ -123,14 +125,14 @@ class ArmMotor{ * @param motorID The motor ID within its WRoboclaw controller * @param n A NodeHandle reference to the constructing Node */ - ArmMotor(const std::string &motorName, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, int64_t offset, ros::NodeHandle &n); + ArmMotor(std::string motorName, unsigned int controllerID, unsigned int motorID, int64_t countsPerRotation, int64_t offset, ros::NodeHandle &n); /** * @brief Gets the encoder value of the motor * * @return uint32_t The current encoder value of the motor */ - uint32_t getEncoderCounts() const; + auto getEncoderCounts() const -> uint32_t; /** * @brief Sends the motor to run to a target encoder value at a given power without blocking @@ -148,7 +150,7 @@ class ArmMotor{ * @param block Specifies whether or not this action should block until it is complete * @return True if the motor had stalled, and false otherwise */ - bool runToTarget(uint32_t targetCounts, float power, bool block); + auto runToTarget(uint32_t targetCounts, float power, bool block) -> bool; /** * @brief Sends the motor to run to a specified position at a given power @@ -157,14 +159,14 @@ class ArmMotor{ * @param power The power to move the motor at (Bounded between [-1, 1]) * @return True if the motor had stalled, and false otherwise */ - bool runToTarget(double rads, float power); + auto runToTarget(double rads, float power) -> bool; /** * @brief Get the current state of the ArmMotor * * @return MotorState The current state of the ArmMotor */ - MotorState getMotorState() const; + auto getMotorState() const -> MotorState; /** * @brief Set the motor power @@ -178,28 +180,28 @@ class ArmMotor{ * * @return double The radian measure of the current motor's position */ - double getRads() const; + auto getRads() const -> double; /** * @brief Get the name of the ArmMotor * * @return std::string The name of the ArmMotor */ - std::string getMotorName() const; + auto getMotorName() const -> std::string; /** * @brief Get the name of the ArmMotor * * @return std::string The controller ID of the ArmMotor */ - unsigned int getControllerID() const; + auto getControllerID() const -> unsigned int; /** * @brief Get the name of the ArmMotor * * @return std::string The motor ID of the ArmMotor */ - unsigned int getMotorID() const; + auto getMotorID() const -> unsigned int; /** @@ -209,7 +211,7 @@ class ArmMotor{ * @return true The motor was within the target tolerance * @return false The motor was outside of the target tolerance */ - bool hasReachedTarget(uint32_t targetCounts) const; + auto hasReachedTarget(uint32_t targetCounts) const -> bool; /** * @brief Checks if the motor is currently within a given tolerance of a target @@ -219,13 +221,13 @@ class ArmMotor{ * @return true The motor was within the target tolerance * @return false The motor was outside the target tolerance */ - bool hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const; + auto hasReachedTarget(uint32_t targetCounts, uint32_t tolerance) const -> bool; /** * @brief Get the most recently set motor power * * @return float Last motor power setting */ - float getPower() const; + auto getPower() const -> float; }; diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index ef600304..360d8f02 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -9,9 +9,9 @@ #include using std::vector; -DifferentialJoint::DifferentialJoint(ArmMotor* leftMotor, ArmMotor* rightMotor, ros::NodeHandle* n) : AbstractJoint(n, 2) { - this->motors.push_back(leftMotor); - this->motors.push_back(rightMotor); +DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n) : AbstractJoint(n, 2) { + this->motors.at(0) = {std::move(leftMotor), 0, 0, "", "", false}; + this->motors.at(1) = {std::move(rightMotor), 0, 0, "", "", false}; } void DifferentialJoint::getJointPositions(vector &motorPositions, vector &target){ diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp index 07606c2a..771c1e5e 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -3,7 +3,7 @@ class DifferentialJoint : public AbstractJoint { public: ~DifferentialJoint(); - DifferentialJoint(ArmMotor *leftMotor, ArmMotor *rightMotor, ros::NodeHandle *n); + DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n); void getMotorPositions(vector &jointPositions, vector &target); void getMotorVelocities(vector &jointVelocities, vector &target); From 54918515ef2671b9ecb8bdd04c215d553ea31818 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 12 Apr 2022 12:09:46 -0500 Subject: [PATCH 07/29] Committing settings for linter. Discussions to be had on whether or not they should be removed prior to merge. --- .gitignore | 1 + .vscode/settings.json | 14 ++++++++++++++ 2 files changed, 15 insertions(+) create mode 100644 .vscode/settings.json diff --git a/.gitignore b/.gitignore index 159790a3..76a69cc1 100644 --- a/.gitignore +++ b/.gitignore @@ -9,6 +9,7 @@ devel/ /.vscode/* # except recommended extensions !/.vscode/extensions.json +!/.vscode/settings.json # fetched external libs # NOTE: these may be removed in the future, tbd diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..48600c15 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,14 @@ +{ + "clang-tidy.checks": [ + "bugprone-*", + "performance-*", + "readability-*", + "modernize-*", + "hicpp-*", + "cppcoreguidelines-*", + "clang-analyzer-*", + "-readability-braces-around-statements", + "-hicpp-braces-around-statements", + "-cppcoreguidelines-non-private-member-variables-in-classes" + ] +} \ No newline at end of file From 4a2739832196ee93743e31c3e0d2b1345a042ab3 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 12 Apr 2022 12:28:55 -0500 Subject: [PATCH 08/29] Continuing to work on the `DifferentialJoint` de-linting --- .../src/DifferentialJoint.cpp | 34 ++++++++++--------- .../src/DifferentialJoint.hpp | 9 ++--- 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index 360d8f02..87273b87 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -9,9 +9,20 @@ #include using std::vector; -DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n) : AbstractJoint(n, 2) { +DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n, + const std::string &pitchTopicName, const std::string &rollTopicName, + const std::string &leftTopicName, const std::string &rightTopicName) : AbstractJoint(n, DEGREES_OF_FREEDOM) { this->motors.at(0) = {std::move(leftMotor), 0, 0, "", "", false}; this->motors.at(1) = {std::move(rightMotor), 0, 0, "", "", false}; + + this->pitchOutputSubscriber = n.subscribe(pitchTopicName + "/output", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffPitchOutput, this); + this->rollOutputSubscriber = n.subscribe(rollTopicName + "/output", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffRollOutput, this); + this->leftOutputPublisher = n.advertise(leftTopicName + "/output", MESSAGE_CACHE_SIZE); + this->rightOutputPublisher = n.advertise(rightTopicName + "/output", MESSAGE_CACHE_SIZE); + this->leftFeedbackSubscriber = n.subscribe(leftTopicName + "/feeback", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffLeftFeedback, this); + this->rightFeedbackSubscriber = n.subscribe(rightTopicName + "/feeback", MESSAGE_CACHE_SIZE, &DifferentialJoint::handoffRightFeedback, this); + this->pitchFeedbackPublisher = n.advertise(pitchTopicName + "/feedback", MESSAGE_CACHE_SIZE); + this->rollFeedbackPublisher = n.advertise(rollTopicName + "/feedback", MESSAGE_CACHE_SIZE); } void DifferentialJoint::getJointPositions(vector &motorPositions, vector &target){ @@ -45,17 +56,6 @@ void DifferentialJoint::getMotorVelocities(vector &jointPositions, vecto return getMotorPositions(jointPositions, target); //deritivate of linear transformation is itself } -void DifferentialJoint::configVelocityHandshake(std::string pitchTopicName, std::string rollTopicName, std::string leftTopicName, std::string rightTopicName){ - this->pitchOutputSubscriber = this->n->subscribe(pitchTopicName + "/output", 1000, &DifferentialJoint::handoffPitchOutput, this); - this->rollOutputSubscriber = this->n->subscribe(rollTopicName + "/output", 1000, &DifferentialJoint::handoffRollOutput, this); - this->leftOutputPublisher = this->n->advertise(leftTopicName + "/output", 1000); - this->rightOutputPublisher = this->n->advertise(rightTopicName + "/output", 1000); - this->leftFeedbackSubscriber = this->n->subscribe(leftTopicName + "/feeback", 1000, &DifferentialJoint::handoffLeftFeedback, this); - this->rightFeedbackSubscriber = this->n->subscribe(rightTopicName + "/feeback", 1000, &DifferentialJoint::handoffRightFeedback, this); - this->pitchFeedbackPublisher = this->n->advertise(pitchTopicName + "/feedback", 1000); - this->rollFeedbackPublisher = this->n->advertise(rollTopicName + "/feedback", 1000); -} - void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64 msg){ this->cachedPitchOutput = msg.data; this->hasNewPitchOutput = true; @@ -71,14 +71,16 @@ void DifferentialJoint::handOffAllOutput(){ if(!this->hasNewPitchOutput || !this->hasNewRollOutput){ return; } vector outputs; - outputs[0] = this->cachedPitchOutput; - outputs[1] = this->cachedRollOutput; + outputs.resize(2); + outputs.at(0) = this->cachedPitchOutput; + outputs.at(1) = this->cachedRollOutput; + //TODO: Still confused as to the purpose of this family of functions getMotorVelocities(outputs, outputs); std_msgs::Float64 msg1 = std_msgs::Float64(); std_msgs::Float64 msg2 = std_msgs::Float64(); - msg1.data = outputs[0]; - msg2.data = outputs[1]; + msg1.data = outputs.at(0); + msg2.data = outputs.at(1); pitchFeedbackPublisher.publish(msg1); rollFeedbackPublisher.publish(msg2); diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp index 771c1e5e..f8e2c1ce 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -2,16 +2,17 @@ class DifferentialJoint : public AbstractJoint { public: - ~DifferentialJoint(); - DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n); + DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n, + const std::string &pitchTopicName, const std::string &rollTopicName, + const std::string &leftTopicName, const std::string &rightTopicName); void getMotorPositions(vector &jointPositions, vector &target); void getMotorVelocities(vector &jointVelocities, vector &target); void getJointPositions(vector &motorPositions, vector &target); - void configVelocityHandshake(std::string pitchTopicName, std::string rollTopicName, std::string leftTopicName, std::string rightTopicName); - private: + static constexpr uint32_t DEGREES_OF_FREEDOM = 2; + static constexpr uint32_t MESSAGE_CACHE_SIZE = 10; // linear transformations works for position and velocity // [0.5 0.5] [left motor ] [pitch] // [ -1 1 ] * [right motor] = [roll ] From 3cfa0c9152f3c848f60d083d622b9f04de479e1f Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 12 Apr 2022 16:20:53 -0500 Subject: [PATCH 09/29] More differential joint refactoring? --- .../src/DifferentialJoint.cpp | 45 ++++++++++--------- .../src/DifferentialJoint.hpp | 18 ++++---- 2 files changed, 33 insertions(+), 30 deletions(-) diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index 87273b87..e6b9f7a3 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -11,7 +11,8 @@ using std::vector; DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n, const std::string &pitchTopicName, const std::string &rollTopicName, - const std::string &leftTopicName, const std::string &rightTopicName) : AbstractJoint(n, DEGREES_OF_FREEDOM) { + const std::string &leftTopicName, const std::string &rightTopicName) + : AbstractJoint(n, DEGREES_OF_FREEDOM) { this->motors.at(0) = {std::move(leftMotor), 0, 0, "", "", false}; this->motors.at(1) = {std::move(rightMotor), 0, 0, "", "", false}; @@ -29,9 +30,10 @@ void DifferentialJoint::getJointPositions(vector &motorPositions, vector // vector positions; // target->reserve(2); - double pitch = motorPositions[0] * this->motorToJointMatrix[0][0] + motorPositions[1]*this->motorToJointMatrix[0][1]; - double roll = motorPositions[1] * this->motorToJointMatrix[1][0] + motorPositions[1]*this->motorToJointMatrix[1][1]; + double pitch = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(0).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(0).at(1); + double roll = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(1).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(1).at(1); + //TODO: Why not return a new vector? Wouldn't this constant resizing be more inefficient than just making/returning a new vector via copy ellision target.push_back(pitch); target.push_back(roll); @@ -43,9 +45,10 @@ void DifferentialJoint::getMotorPositions(vector &jointPositions, vector // std::unique_ptr> positions = std::make_unique>(2); // target->reserve(2); - double left = jointPositions[0] * this->jointToMotorMatrix[0][0] + jointPositions[1]*this->jointToMotorMatrix[0][1]; - double right = jointPositions[1] * this->jointToMotorMatrix[1][0] + jointPositions[1]*this->jointToMotorMatrix[1][1]; + double left = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(0).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(0).at(1); + double right = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(1).at(0) + jointPositions.at(1)* JOINT_TO_MOTOR_MATRIX.at(1).at(1); + // TODO: See line 36 target.push_back(left); target.push_back(right); @@ -56,13 +59,13 @@ void DifferentialJoint::getMotorVelocities(vector &jointPositions, vecto return getMotorPositions(jointPositions, target); //deritivate of linear transformation is itself } -void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64 msg){ - this->cachedPitchOutput = msg.data; +void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64::ConstPtr &msg){ + this->cachedPitchOutput = msg->data; this->hasNewPitchOutput = true; handOffAllOutput(); } -void DifferentialJoint::handoffRollOutput(const std_msgs::Float64 msg){ - this->cachedRollOutput = msg.data; +void DifferentialJoint::handoffRollOutput(const std_msgs::Float64::ConstPtr &msg){ + this->cachedRollOutput = msg->data; this->hasNewRollOutput = true; handOffAllOutput(); } @@ -77,8 +80,8 @@ void DifferentialJoint::handOffAllOutput(){ //TODO: Still confused as to the purpose of this family of functions getMotorVelocities(outputs, outputs); - std_msgs::Float64 msg1 = std_msgs::Float64(); - std_msgs::Float64 msg2 = std_msgs::Float64(); + std_msgs::Float64 msg1; + std_msgs::Float64 msg2; msg1.data = outputs.at(0); msg2.data = outputs.at(1); pitchFeedbackPublisher.publish(msg1); @@ -92,13 +95,13 @@ void DifferentialJoint::handOffAllOutput(){ } -void DifferentialJoint::handoffRightFeedback(const std_msgs::Float64 msg){ - this->cachedRightFeedback = msg.data; +void DifferentialJoint::handoffRightFeedback(const std_msgs::Float64::ConstPtr &msg){ + this->cachedRightFeedback = msg->data; this->hasNewRightFeedback = true; handOffAllFeedback(); } -void DifferentialJoint::handoffLeftFeedback(const std_msgs::Float64 msg){ - this->cachedLeftFeedback = msg.data; +void DifferentialJoint::handoffLeftFeedback(const std_msgs::Float64::ConstPtr &msg){ + this->cachedLeftFeedback = msg->data; this->hasNewLeftFeedback = true; handOffAllFeedback(); } @@ -107,14 +110,14 @@ void DifferentialJoint::handOffAllFeedback(){ if(!this->hasNewLeftFeedback || !this->hasNewRightFeedback){ return; } vector outputs; - outputs[0] = this->cachedLeftFeedback; - outputs[1] = this->cachedRightFeedback; + outputs.at(0) = this->cachedLeftFeedback; + outputs.at(1) = this->cachedRightFeedback; getMotorVelocities(outputs, outputs); - std_msgs::Float64 msg1 = std_msgs::Float64(); - std_msgs::Float64 msg2 = std_msgs::Float64(); - msg1.data = outputs[0]; - msg2.data = outputs[1]; + std_msgs::Float64 msg1; + std_msgs::Float64 msg2; + msg1.data = outputs.at(0); + msg2.data = outputs.at(1); pitchFeedbackPublisher.publish(msg1); rollFeedbackPublisher.publish(msg2); diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp index f8e2c1ce..6c460ca0 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -6,9 +6,9 @@ class DifferentialJoint : public AbstractJoint { const std::string &pitchTopicName, const std::string &rollTopicName, const std::string &leftTopicName, const std::string &rightTopicName); - void getMotorPositions(vector &jointPositions, vector &target); - void getMotorVelocities(vector &jointVelocities, vector &target); - void getJointPositions(vector &motorPositions, vector &target); + void getMotorPositions(vector &jointPositions, vector &target) override; + void getMotorVelocities(vector &jointVelocities, vector &target) override; + void getJointPositions(vector &motorPositions, vector &target) override; private: static constexpr uint32_t DEGREES_OF_FREEDOM = 2; @@ -17,14 +17,14 @@ class DifferentialJoint : public AbstractJoint { // [0.5 0.5] [left motor ] [pitch] // [ -1 1 ] * [right motor] = [roll ] // double motorToJointMatrix[2][2] = {{0.5, 0.5}, {-1, 1}}; - double motorToJointMatrix[2][2] = {{1, 0}, {0, 1}}; + static constexpr std::array, 2> MOTOR_TO_JOINT_MATRIX{{{1, 0}, {0, 1}}}; // [1 -0.5] [pitch] [left motor ] // [1 0.5] * [roll ] = [right motor] // double jointToMotorMatrix[2][2] = {{1, 0.5}, {1, -0.5}}; - double jointToMotorMatrix[2][2] = {{1, 0}, {0, 1.0}}; + static constexpr std::array, 2> JOINT_TO_MOTOR_MATRIX{{{1, 0}, {0, 1}}}; - void handoffPitchOutput(std_msgs::Float64); - void handoffRollOutput(std_msgs::Float64); + void handoffPitchOutput(const std_msgs::Float64::ConstPtr&); + void handoffRollOutput(const std_msgs::Float64::ConstPtr&); void handOffAllOutput(); ros::Subscriber pitchOutputSubscriber; ros::Subscriber rollOutputSubscriber; @@ -35,8 +35,8 @@ class DifferentialJoint : public AbstractJoint { bool hasNewPitchOutput = false; bool hasNewRollOutput = false; - void handoffLeftFeedback(std_msgs::Float64); - void handoffRightFeedback(std_msgs::Float64); + void handoffLeftFeedback(const std_msgs::Float64::ConstPtr&); + void handoffRightFeedback(const std_msgs::Float64::ConstPtr&); void handOffAllFeedback(); ros::Subscriber leftFeedbackSubscriber; ros::Subscriber rightFeedbackSubscriber; From b1836c7edabd386575b075e503d7af1dbff07842 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 12 Apr 2022 17:42:30 -0500 Subject: [PATCH 10/29] Adding more TODO remarks and modifying method signatures to add `const` to parameters. --- src/wr_control_drive_arm/src/AbstractJoint.hpp | 6 +++--- src/wr_control_drive_arm/src/DifferentialJoint.cpp | 9 +++++---- src/wr_control_drive_arm/src/DifferentialJoint.hpp | 6 +++--- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index c187eff4..f4c10d05 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -18,9 +18,9 @@ class AbstractJoint { AbstractJoint(ros::NodeHandle &n, int numMotors); // never used, need to be defined for compiler v-table - virtual void getMotorPositions(vector &jointPositions, vector &target) = 0; - virtual void getMotorVelocities(vector &joinVelocities, vector &target) = 0; - virtual void getJointPositions(vector &motorPositions, vector &target) = 0; + virtual void getMotorPositions(const vector &jointPositions, vector &target) = 0; + virtual void getMotorVelocities(const vector &joinVelocities, vector &target) = 0; + virtual void getJointPositions(const vector &motorPositions, vector &target) = 0; auto getDegreesOfFreedom() const -> unsigned int; diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index e6b9f7a3..803565e0 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -26,21 +26,21 @@ DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::u this->rollFeedbackPublisher = n.advertise(rollTopicName + "/feedback", MESSAGE_CACHE_SIZE); } -void DifferentialJoint::getJointPositions(vector &motorPositions, vector &target){ +void DifferentialJoint::getJointPositions(const vector &motorPositions, vector &target){ // vector positions; // target->reserve(2); double pitch = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(0).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(0).at(1); double roll = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(1).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(1).at(1); - //TODO: Why not return a new vector? Wouldn't this constant resizing be more inefficient than just making/returning a new vector via copy ellision + //TODO: Why not return a new vector? Wouldn't this constant resizing be more inefficient than just making/returning a new vector via copy ellision? target.push_back(pitch); target.push_back(roll); // return positions; } -void DifferentialJoint::getMotorPositions(vector &jointPositions, vector &target){ +void DifferentialJoint::getMotorPositions(const vector &jointPositions, vector &target){ // std::unique_ptr> positions = std::make_unique>(2); // target->reserve(2); @@ -55,7 +55,7 @@ void DifferentialJoint::getMotorPositions(vector &jointPositions, vector // return std::move(positions); } -void DifferentialJoint::getMotorVelocities(vector &jointPositions, vector &target){ +void DifferentialJoint::getMotorVelocities(const vector &jointPositions, vector &target){ return getMotorPositions(jointPositions, target); //deritivate of linear transformation is itself } @@ -70,6 +70,7 @@ void DifferentialJoint::handoffRollOutput(const std_msgs::Float64::ConstPtr &msg handOffAllOutput(); } +// TODO: Do these ouputs need to be synced? We're just referring to the motors individually, maybe we can transform and send the data right away void DifferentialJoint::handOffAllOutput(){ if(!this->hasNewPitchOutput || !this->hasNewRollOutput){ return; } diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp index 6c460ca0..2ec82b64 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -6,9 +6,9 @@ class DifferentialJoint : public AbstractJoint { const std::string &pitchTopicName, const std::string &rollTopicName, const std::string &leftTopicName, const std::string &rightTopicName); - void getMotorPositions(vector &jointPositions, vector &target) override; - void getMotorVelocities(vector &jointVelocities, vector &target) override; - void getJointPositions(vector &motorPositions, vector &target) override; + void getMotorPositions(const vector &jointPositions, vector &target) override; + void getMotorVelocities(const vector &jointVelocities, vector &target) override; + void getJointPositions(const vector &motorPositions, vector &target) override; private: static constexpr uint32_t DEGREES_OF_FREEDOM = 2; From af2e80ffdb1a794fa97548f10a3cd3c7ba9f06f5 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Tue, 12 Apr 2022 17:43:45 -0500 Subject: [PATCH 11/29] Formatting --- src/wr_control_drive_arm/src/DifferentialJoint.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index 803565e0..a9672725 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -46,7 +46,7 @@ void DifferentialJoint::getMotorPositions(const vector &jointPositions, // target->reserve(2); double left = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(0).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(0).at(1); - double right = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(1).at(0) + jointPositions.at(1)* JOINT_TO_MOTOR_MATRIX.at(1).at(1); + double right = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(1).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(1).at(1); // TODO: See line 36 target.push_back(left); From 7178e65beff23b0447d0f52785e18925707081ea Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Thu, 14 Apr 2022 17:19:57 -0700 Subject: [PATCH 12/29] Refactoring --- .vscode/settings.json | 80 +++++++++++++++++++- src/wr_control_drive_arm/src/SimpleJoint.cpp | 23 ++---- src/wr_control_drive_arm/src/SimpleJoint.hpp | 27 ++----- 3 files changed, 90 insertions(+), 40 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 48600c15..7e766987 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -10,5 +10,83 @@ "-readability-braces-around-statements", "-hicpp-braces-around-statements", "-cppcoreguidelines-non-private-member-variables-in-classes" - ] + ], + "python.autoComplete.extraPaths": [ + "/home/arthur/WiscRobotics/WRover_Software/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/home/arthur/WiscRobotics/WRover_Software/devel/lib/python3/dist-packages", + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "bit": "cpp" + } } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.cpp b/src/wr_control_drive_arm/src/SimpleJoint.cpp index b846f219..ead1e4e1 100644 --- a/src/wr_control_drive_arm/src/SimpleJoint.cpp +++ b/src/wr_control_drive_arm/src/SimpleJoint.cpp @@ -1,5 +1,3 @@ -#ifndef SIMPLE_JOINT_GUARD -#define SIMPLE_JOINT_GUARD /** * @file AbstractJoint.cpp * @author Nichols Underwood @@ -9,11 +7,11 @@ #include "SimpleJoint.hpp" -SimpleJoint::SimpleJoint(ArmMotor* motor, ros::NodeHandle *n) : AbstractJoint(n, 1) { - this->motors.push_back(motor); +SimpleJoint::SimpleJoint(std::unique_ptr motor, ros::NodeHandle &n) : AbstractJoint(n, 1) { + this->motors.at(0) = MotorHandler{std::move(motor), 0, 0, "", "", false}; } -void SimpleJoint::getJointPositions(vector &motorPositions, vector &target){ +void SimpleJoint::getJointPositions(const vector &motorPositions, vector &target){ // vector positions = *(new vector()); // makes the red lines go away // target->reserve(1); @@ -22,7 +20,7 @@ void SimpleJoint::getJointPositions(vector &motorPositions, vector &jointPositions, vector &target){ +void SimpleJoint::getMotorPositions(const vector &jointPositions, vector &target){ target.reserve(1); double position = jointPositions[0]; @@ -30,19 +28,10 @@ void SimpleJoint::getMotorPositions(vector &jointPositions, vector &jointPositions, vector &target){ +void SimpleJoint::getMotorVelocities(const vector &jointPositions, vector &target){ // target->reserve(1); target.push_back(jointPositions[0]); // return setpoints; -} - -void SimpleJoint::handoffOutput(const std_msgs::Float64 msg){ - this->motorOutputPublisher.publish(msg); -} - -void SimpleJoint::handoffFeedback(const std_msgs::Float64 msg){ - this->jointFeedbackPublisher.publish(msg); -} -#endif \ No newline at end of file +} \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.hpp b/src/wr_control_drive_arm/src/SimpleJoint.hpp index 9be7198d..b354b5c1 100644 --- a/src/wr_control_drive_arm/src/SimpleJoint.hpp +++ b/src/wr_control_drive_arm/src/SimpleJoint.hpp @@ -1,28 +1,11 @@ #include "AbstractJoint.hpp" + using std::vector; class SimpleJoint : public AbstractJoint { public: - ~SimpleJoint(); - SimpleJoint(ArmMotor* motor, ros::NodeHandle* n); - - int a(){return 2;} - int b(){return 3;} - - void getMotorPositions(vector &jointPositions, vector &target); - void getMotorVelocities(vector &ointVelocities, vector &target); - void getJointPositions(vector &motorPositions, vector &target); - - void configMotor(ArmMotor* motor); - void configVelocityHandshake(std::string jointTopicName, std::string motorTopicName); - void handoffOutput(std_msgs::Float64); - void handoffFeedback(std_msgs::Float64); - - private: - ros::Subscriber jointOutputSubscriber; - ros::Publisher motorOutputPublisher; - - ros::Subscriber motorFeedbackSubscriber; - ros::Publisher jointFeedbackPublisher; - + SimpleJoint(std::unique_ptr motor, ros::NodeHandle& n); + void getMotorPositions(const vector &jointPositions, vector &target) override; + void getMotorVelocities(const vector &ointVelocities, vector &target) override; + void getJointPositions(const vector &motorPositions, vector &target) override; }; From 9360c3305684798a2d3a9d2c68fe9508f3cfffde Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Thu, 14 Apr 2022 17:21:29 -0700 Subject: [PATCH 13/29] Undo changes to settings.json --- .vscode/settings.json | 80 +------------------------------------------ 1 file changed, 1 insertion(+), 79 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 7e766987..48600c15 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -10,83 +10,5 @@ "-readability-braces-around-statements", "-hicpp-braces-around-statements", "-cppcoreguidelines-non-private-member-variables-in-classes" - ], - "python.autoComplete.extraPaths": [ - "/home/arthur/WiscRobotics/WRover_Software/devel/lib/python3/dist-packages", - "/opt/ros/noetic/lib/python3/dist-packages" - ], - "python.analysis.extraPaths": [ - "/home/arthur/WiscRobotics/WRover_Software/devel/lib/python3/dist-packages", - "/opt/ros/noetic/lib/python3/dist-packages" - ], - "files.associations": { - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "array": "cpp", - "atomic": "cpp", - "strstream": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "complex": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "map": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "set": "cpp", - "string": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "hash_map": "cpp", - "hash_set": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "ostream": "cpp", - "shared_mutex": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "variant": "cpp", - "bit": "cpp" - } + ] } \ No newline at end of file From d137265cc6507e16769829cd733385aa474e7cf4 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 18 Apr 2022 18:10:11 -0500 Subject: [PATCH 14/29] AbstractJoint signature changes. --- src/wr_control_drive_arm/src/AbstractJoint.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index f4c10d05..d6394a4e 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -18,9 +18,9 @@ class AbstractJoint { AbstractJoint(ros::NodeHandle &n, int numMotors); // never used, need to be defined for compiler v-table - virtual void getMotorPositions(const vector &jointPositions, vector &target) = 0; - virtual void getMotorVelocities(const vector &joinVelocities, vector &target) = 0; - virtual void getJointPositions(const vector &motorPositions, vector &target) = 0; + virtual vector getMotorPositions(const vector &jointPositions) = 0; + virtual vector getMotorVelocities(const vector &joinVelocities) = 0; + virtual vector getJointPositions(const vector &motorPositions) = 0; auto getDegreesOfFreedom() const -> unsigned int; From 73b61ef8728aa5085a69926fd37bd6c93ccf8f81 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 18 Apr 2022 18:13:21 -0500 Subject: [PATCH 15/29] AbstractJoint de-linting --- src/wr_control_drive_arm/src/AbstractJoint.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index d6394a4e..9bb61c00 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -18,9 +18,9 @@ class AbstractJoint { AbstractJoint(ros::NodeHandle &n, int numMotors); // never used, need to be defined for compiler v-table - virtual vector getMotorPositions(const vector &jointPositions) = 0; - virtual vector getMotorVelocities(const vector &joinVelocities) = 0; - virtual vector getJointPositions(const vector &motorPositions) = 0; + virtual auto getMotorPositions(const vector &jointPositions) -> vector = 0; + virtual auto getMotorVelocities(const vector &joinVelocities) -> vector = 0; + virtual auto getJointPositions(const vector &motorPositions) -> vector = 0; auto getDegreesOfFreedom() const -> unsigned int; From d96df64b3438eaba9d74b2eb899e2905a3cd4b3d Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 18 Apr 2022 18:33:17 -0500 Subject: [PATCH 16/29] DifferentialJoint de-linted. --- .../src/DifferentialJoint.cpp | 19 ++++++++----------- .../src/DifferentialJoint.hpp | 6 +++--- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index a9672725..38a2bb02 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -26,7 +26,7 @@ DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::u this->rollFeedbackPublisher = n.advertise(rollTopicName + "/feedback", MESSAGE_CACHE_SIZE); } -void DifferentialJoint::getJointPositions(const vector &motorPositions, vector &target){ +auto DifferentialJoint::getJointPositions(const vector &motorPositions) -> vector{ // vector positions; // target->reserve(2); @@ -34,13 +34,12 @@ void DifferentialJoint::getJointPositions(const vector &motorPositions, double roll = motorPositions.at(0) * MOTOR_TO_JOINT_MATRIX.at(1).at(0) + motorPositions.at(1) * MOTOR_TO_JOINT_MATRIX.at(1).at(1); //TODO: Why not return a new vector? Wouldn't this constant resizing be more inefficient than just making/returning a new vector via copy ellision? - target.push_back(pitch); - target.push_back(roll); + return {pitch, roll}; // return positions; } -void DifferentialJoint::getMotorPositions(const vector &jointPositions, vector &target){ +auto DifferentialJoint::getMotorPositions(const vector &jointPositions) -> vector{ // std::unique_ptr> positions = std::make_unique>(2); // target->reserve(2); @@ -48,15 +47,13 @@ void DifferentialJoint::getMotorPositions(const vector &jointPositions, double left = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(0).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(0).at(1); double right = jointPositions.at(0) * JOINT_TO_MOTOR_MATRIX.at(1).at(0) + jointPositions.at(1) * JOINT_TO_MOTOR_MATRIX.at(1).at(1); - // TODO: See line 36 - target.push_back(left); - target.push_back(right); + return {left, right}; // return std::move(positions); } -void DifferentialJoint::getMotorVelocities(const vector &jointPositions, vector &target){ - return getMotorPositions(jointPositions, target); //deritivate of linear transformation is itself +auto DifferentialJoint::getMotorVelocities(const vector &jointPositions) -> vector{ + return getMotorPositions(jointPositions); //deritivate of linear transformation is itself } void DifferentialJoint::handoffPitchOutput(const std_msgs::Float64::ConstPtr &msg){ @@ -79,7 +76,7 @@ void DifferentialJoint::handOffAllOutput(){ outputs.at(0) = this->cachedPitchOutput; outputs.at(1) = this->cachedRollOutput; //TODO: Still confused as to the purpose of this family of functions - getMotorVelocities(outputs, outputs); + outputs = getMotorVelocities(outputs); std_msgs::Float64 msg1; std_msgs::Float64 msg2; @@ -113,7 +110,7 @@ void DifferentialJoint::handOffAllFeedback(){ vector outputs; outputs.at(0) = this->cachedLeftFeedback; outputs.at(1) = this->cachedRightFeedback; - getMotorVelocities(outputs, outputs); + outputs = getMotorVelocities(outputs); std_msgs::Float64 msg1; std_msgs::Float64 msg2; diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp index 2ec82b64..d71fddd3 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -6,9 +6,9 @@ class DifferentialJoint : public AbstractJoint { const std::string &pitchTopicName, const std::string &rollTopicName, const std::string &leftTopicName, const std::string &rightTopicName); - void getMotorPositions(const vector &jointPositions, vector &target) override; - void getMotorVelocities(const vector &jointVelocities, vector &target) override; - void getJointPositions(const vector &motorPositions, vector &target) override; + auto getMotorPositions(const vector &jointPositions) -> vector override; + auto getMotorVelocities(const vector &jointVelocities) -> vector override; + auto getJointPositions(const vector &motorPositions) -> vector override; private: static constexpr uint32_t DEGREES_OF_FREEDOM = 2; From 6156f6e591256c9b4654f2e88dff0b8424dc509b Mon Sep 17 00:00:00 2001 From: Arthur Wang Date: Mon, 18 Apr 2022 16:47:03 -0700 Subject: [PATCH 17/29] De-linting SimpleJoint --- src/wr_control_drive_arm/src/SimpleJoint.cpp | 25 +++++--------------- src/wr_control_drive_arm/src/SimpleJoint.hpp | 6 ++--- 2 files changed, 9 insertions(+), 22 deletions(-) diff --git a/src/wr_control_drive_arm/src/SimpleJoint.cpp b/src/wr_control_drive_arm/src/SimpleJoint.cpp index ead1e4e1..8617b2a0 100644 --- a/src/wr_control_drive_arm/src/SimpleJoint.cpp +++ b/src/wr_control_drive_arm/src/SimpleJoint.cpp @@ -11,27 +11,14 @@ SimpleJoint::SimpleJoint(std::unique_ptr motor, ros::NodeHandle &n) : this->motors.at(0) = MotorHandler{std::move(motor), 0, 0, "", "", false}; } -void SimpleJoint::getJointPositions(const vector &motorPositions, vector &target){ - // vector positions = *(new vector()); // makes the red lines go away - - // target->reserve(1); - target.push_back(motorPositions[0]); - - // return positions; +auto SimpleJoint::getMotorPositions(const vector &jointPositions) -> vector { + return {jointPositions[0]}; } -void SimpleJoint::getMotorPositions(const vector &jointPositions, vector &target){ - - target.reserve(1); - double position = jointPositions[0]; - target.push_back(position); - // return positions; +auto SimpleJoint::getMotorVelocities(const vector &jointVelocities) -> vector { + return {jointVelocities[0]}; } -void SimpleJoint::getMotorVelocities(const vector &jointPositions, vector &target){ - - // target->reserve(1); - target.push_back(jointPositions[0]); - - // return setpoints; +auto SimpleJoint::getJointPositions(const vector &jointPositions) -> vector { + return {jointPositions[0]}; } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/SimpleJoint.hpp b/src/wr_control_drive_arm/src/SimpleJoint.hpp index b354b5c1..5819a60d 100644 --- a/src/wr_control_drive_arm/src/SimpleJoint.hpp +++ b/src/wr_control_drive_arm/src/SimpleJoint.hpp @@ -5,7 +5,7 @@ using std::vector; class SimpleJoint : public AbstractJoint { public: SimpleJoint(std::unique_ptr motor, ros::NodeHandle& n); - void getMotorPositions(const vector &jointPositions, vector &target) override; - void getMotorVelocities(const vector &ointVelocities, vector &target) override; - void getJointPositions(const vector &motorPositions, vector &target) override; + auto getMotorPositions(const vector &jointPositions) -> vector override; + auto getMotorVelocities(const vector &joinVelocities) -> vector override; + auto getJointPositions(const vector &motorPositions) -> vector override; }; From 6dc186e6f544883550090ee4d8a1bfed8b4164e9 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 18 Apr 2022 20:16:15 -0500 Subject: [PATCH 18/29] Finished first-pass linting, not building --- .../src/ArmControlSystem.cpp | 118 ++++++++---------- 1 file changed, 49 insertions(+), 69 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 032275b8..03561dc2 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -27,7 +27,11 @@ using XmlRpc::XmlRpcValue; /** * @brief Refresh rate of ros::Rate */ -constexpr float CLOCK_RATE = 2; +constexpr float CLOCK_RATE = 50; + +constexpr double IK_WARN_RATE = 1.0/2; + +constexpr double JOINT_SAFETY_MAX_SPEED = 0.3; /** * @brief Nessage cache size of publisher @@ -42,8 +46,8 @@ constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0; /** * @brief Defines space for all ArmMotor references */ -constexpr int NUM_MOTORS = 6; -std::array, NUM_MOTORS> motors; +// constexpr int NUM_MOTORS = 6; +// std::array, NUM_MOTORS> motors; // ArmMotor *motors[numMotors]; /** @@ -73,23 +77,6 @@ std::atomic_bool IKEnabled{true}; */ ros::ServiceClient enableServiceClient; -/** - * @brief converts a roll and pitch to joint motor positions - * - * @param roll the roll of the joint in radians - * @param pitch the pitch of the joint in radians - * @return a 2D vector with motor positions in radians - */ -std::vector convertJointSpacetoEncoderSpace(double roll, double pitch){ - std::vector encoderSpace(2); - - //TODO: add matrix transformation, implement joint in mock.py - encoderSpace[0] = roll; - encoderSpace[1] = pitch; - - return encoderSpace; -} - /** * @brief sets a target position and pulls info from motor * @@ -99,7 +86,7 @@ std::vector convertJointSpacetoEncoderSpace(double roll, double pitch){ * @param motor a pointer to the motor * @return if the motor has reached its target */ -bool configJointSetpoint(AbstractJoint* joint, int degreeIndex, double target, float velocity){ +auto configJointSetpoint(const std::unique_ptr &joint, int degreeIndex, double target, float velocity) -> bool{ // Each motor should run to its respective target position at a fixed speed // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)setAborted(); - ROS_WARN("IK is disabled"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) + ROS_WARN_THROTTLE(IK_WARN_RATE, "IK is disabled"); // NOLINT(hicpp-no-array-decay,hicpp,hicpp-vararg,cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) return; } std::cout << "start exec: " << goal->trajectory.points.size() << std::endl; // For each point in the trajectory execution sequence... - for(int i = 0; i < goal->trajectory.points.size(); i++){ - std::cout << "PID to trajectory point " << i << "/" << goal->trajectory.points.size() << std::endl; - // Capture the current goal for easy reference - trajectory_msgs::JointTrajectoryPoint currTargetPosition = goal->trajectory.points[i]; - + for(const auto &currTargetPosition : goal->trajectory.points){ // Track whether or not the current position is done bool hasPositionFinished = false; // Keep max loop rate at 50 Hz - ros::Rate loop(CLOCK_RATE); + ros::Rate loop{CLOCK_RATE}; + + const double VELOCITY_MAX = abs(*std::max_element( + currTargetPosition.velocities.begin(), + currTargetPosition.velocities.end(), + [](double a, double b) -> bool{ return abs(a)getDegreesOfFreedom(); i++){ + joint->configSetpoint(i, currTargetPosition.positions[currItr], JOINT_SAFETY_MAX_SPEED*currTargetPosition.velocities[currItr]/VELOCITY_MAX); + currItr++; + } + } // While the current position is not complete yet... while(!hasPositionFinished){ // Assume the current action is done until proven otherwise hasPositionFinished = true; // Create the Joint State message for the current update cycle - double velMax = abs(*std::max_element( - currTargetPosition.velocities.begin(), - currTargetPosition.velocities.end(), - [](double a, double b) { return abs(a)getDegreesOfFreedom(); k++){ // Each motor should run to its respective target position at a fixed speed // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters - double targetPos = currTargetPosition.positions[motorIndex]; - float currPower = 0.1 * currTargetPosition.velocities[j]/velMax; - - bool hasMotorFinished = configJointSetpoint(joint, motorIndex-jointIndex, targetPos, currPower); - hasPositionFinished &= hasMotorFinished; - motorIndex++; + hasPositionFinished &= joint->getMotor(k)->getMotorState() == MotorState::STOP; // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power // std::cout<getMotor(k)->getMotorName()<<":"<getMotorName().length())<getRads()<getEncoderCounts()<setPower(0.0F); + for(const auto &joint : joints){ + for(int i = 0; i < joint->getDegreesOfFreedom(); i++){ + joint->getMotor(i)->setPower(0.F); + } } as->setSucceeded(); @@ -209,9 +191,11 @@ void publish(const ros::TimerEvent &event){ std::vector positions; sensor_msgs::JointState js_msg; - for (auto &motor : motors){ - names.push_back(motor->getMotorName()); - positions.push_back(motor->getRads()); + for(const auto &joint : joints){ + for(int i = 0; i < joint->getDegreesOfFreedom(); i++){ + names.push_back(joint->getMotor(i)->getMotorName()); + positions.push_back(joint->getMotor(i)->getRads()); + } } positions[4] = positions[5] + positions[4] / 2; @@ -229,7 +213,7 @@ void publish(const ros::TimerEvent &event){ * @param argv The given program arguments * @return int The status code on exiting the program */ -int main(int argc, char** argv) +auto main(int argc, char** argv) ->int { std::cout << "start main" << std::endl; // Initialize the current node as ArmControlSystem @@ -243,26 +227,22 @@ int main(int argc, char** argv) // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node // TODO: fix - motors.at(0) = new ArmMotor("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); - motors[1] = new ArmMotor("forearm_roll", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); - motors[2] = new ArmMotor("shoulder", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); - motors[3] = new ArmMotor("turntable", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n); - motors[4] = new ArmMotor("wrist_pitch", 2, 0, static_cast(encParams[4]["counts_per_rotation"]), static_cast(encParams[4]["offset"]), n); - motors[5] = new ArmMotor("wrist_roll", 2, 1, static_cast(encParams[5]["counts_per_rotation"]), static_cast(encParams[5]["offset"]), n); + auto elbow = std::make_unique("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); + auto forearmRoll = std::make_unique("forearm_roll", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); + auto shoulder = std::make_unique("shoulder", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); + auto turntable = std::make_unique("turntable", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n); + auto wristLeft = std::make_unique("wrist_pitch", 2, 0, static_cast(encParams[4]["counts_per_rotation"]), static_cast(encParams[4]["offset"]), n); + auto wristRight = std::make_unique("wrist_roll", 2, 1, static_cast(encParams[5]["counts_per_rotation"]), static_cast(encParams[5]["offset"]), n); std::cout << "init motors" << std::endl; // Initialize all Joints // TODO: fix - joints.at(0) = new SimpleJoint(motors[0], &n); - joints[1] = new SimpleJoint(motors[1], &n); - joints[2] = new SimpleJoint(motors[2], &n); - joints[3] = new SimpleJoint(motors[3], &n); - joints[4] = new SimpleJoint(motors[4], &n); - DifferentialJoint* temp = new DifferentialJoint(motors[4], motors[5], &n); - temp->configVelocityHandshake("/control/arm/5/roll", "/control/arm/5/pitch", "/control/arm/20/", "/control/arm/21/"); - + joints.at(0) = std::make_unique(std::move(elbow), n); + joints.at(1) =std::make_unique(std::move(forearmRoll), n); + joints.at(2) = std::make_unique(std::move(shoulder), n); + joints.at(3) = std::make_unique(std::move(turntable), n); + joints.at(4) = std::make_unique(std::move(wristLeft), std::move(wristRight), n, "/control/arm/5/pitch", "/control/arm/5/roll", "/control/arm/20/", "/control/arm/21/"); std::cout << "init joints" << std::endl; - joints[5] = temp; // Initialize the Joint State Data Publisher jointStatePublisher = n.advertise("/control/arm_joint_states", MESSAGE_CACHE_SIZE); From e3d08a1a08a4e7eb1283ad24301f770fb9f45491 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Mon, 18 Apr 2022 22:03:33 -0500 Subject: [PATCH 19/29] Updating to C++17 to resolve static storage issues. --- src/wr_control_drive_arm/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index d3f0ce92..2a565490 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -1,8 +1,8 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(wr_control_drive_arm) ## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +add_compile_options(-std=c++17) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) From c15e338dccc605c827a7cf41cc21243683022a4f Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 20 Apr 2022 15:49:57 -0500 Subject: [PATCH 20/29] Removing unused method/minor refactoring. --- .../src/ArmControlSystem.cpp | 25 ++----------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 03561dc2..71d37dcb 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -77,27 +77,6 @@ std::atomic_bool IKEnabled{true}; */ ros::ServiceClient enableServiceClient; -/** - * @brief sets a target position and pulls info from motor - * - * @param names a vector with motor joint names - * @param positions a vector with positions in radians - * @param target the target radian value - * @param motor a pointer to the motor - * @return if the motor has reached its target - */ -auto configJointSetpoint(const std::unique_ptr &joint, int degreeIndex, double target, float velocity) -> bool{ - // Each motor should run to its respective target position at a fixed speed - // TODO: this speed should be capped/dynamic to reflect the input joint velocity parameters - // velMax = abs(*std::max_element(currTargetPosition.velocities.begin(), currTargetPosition.velocities.end(), [](double a, double b) {return abs(a)configSetpoint(degreeIndex, target, 0); - // The position has only finished if every motor is STOPped - return joint->getMotor(degreeIndex)->getMotorState() == MotorState::STOP; -} - /** * @brief Perform the given action as interpreted as moving the arm joints to specified positions * @@ -186,7 +165,7 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server /** * @brief publishes the arm's position */ -void publish(const ros::TimerEvent &event){ +void publishJointStates(const ros::TimerEvent &event){ std::vector names; std::vector positions; sensor_msgs::JointState js_msg; @@ -253,7 +232,7 @@ auto main(int argc, char** argv) ->int server.start(); std::cout << "server started" << std::endl; - ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publish); + ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publishJointStates); enableServiceServer = n.advertiseService("start_IK", static_cast>( From b33fb6fd378fb38ae983fe8a6c3bdba03d7eecaf Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 20 Apr 2022 15:50:32 -0500 Subject: [PATCH 21/29] Able to roll back C++ version to auto-deduced 14, resolved linker error. --- src/wr_control_drive_arm/CMakeLists.txt | 2 +- src/wr_control_drive_arm/src/DifferentialJoint.cpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index 2a565490..c9d43c92 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.8) project(wr_control_drive_arm) ## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++17) +# add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index 38a2bb02..c71d7753 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -9,6 +9,9 @@ #include using std::vector; +constexpr std::array, 2> DifferentialJoint::MOTOR_TO_JOINT_MATRIX; +constexpr std::array, 2> DifferentialJoint::JOINT_TO_MOTOR_MATRIX; + DifferentialJoint::DifferentialJoint(std::unique_ptr leftMotor, std::unique_ptr rightMotor, ros::NodeHandle &n, const std::string &pitchTopicName, const std::string &rollTopicName, const std::string &leftTopicName, const std::string &rightTopicName) From fc94816571c0ab1e78ff90553e20f1aefacc38a5 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Wed, 20 Apr 2022 19:26:39 -0500 Subject: [PATCH 22/29] Arm Encoder Tuning --- .../config/encoder_parameters.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/wr_control_drive_arm/config/encoder_parameters.yaml b/src/wr_control_drive_arm/config/encoder_parameters.yaml index 9421827f..76e5cbd2 100644 --- a/src/wr_control_drive_arm/config/encoder_parameters.yaml +++ b/src/wr_control_drive_arm/config/encoder_parameters.yaml @@ -3,17 +3,17 @@ encoder_parameters: - counts_per_rotation: -2048 offset: 736 # forearm_roll - - counts_per_rotation: 2048 - offset: 0 + - counts_per_rotation: -2048 + offset: 600 # shoulder - counts_per_rotation: -6144 - offset: 460 + offset: 1450 # turntable - counts_per_rotation: 2048 - offset: 1111 + offset: 736 # wrist_pitch - counts_per_rotation: 2048 - offset: 350 + offset: 696 # wrist_roll - counts_per_rotation: 2048 - offset: 1023 + offset: 1045 From c020ab7d9315bd4a2cde9464bd5352e2fc7499c0 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 20 Apr 2022 19:32:38 -0500 Subject: [PATCH 23/29] Updating wrist tracking logic. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 71d37dcb..46016e77 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -177,7 +177,10 @@ void publishJointStates(const ros::TimerEvent &event){ } } - positions[4] = positions[5] + positions[4] / 2; + const auto left = positions.at(4); + const auto right = positions.at(5); + positions[4] = left + right / 2; + positions[5] = left - right; js_msg.name = names; js_msg.position = positions; From 5a4ecf238277070f1516d7bb8164c63300471de7 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Wed, 20 Apr 2022 19:54:09 -0500 Subject: [PATCH 24/29] Updating wrist tracking logic. --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 46016e77..2c9ebd67 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -178,9 +178,9 @@ void publishJointStates(const ros::TimerEvent &event){ } const auto left = positions.at(4); - const auto right = positions.at(5); - positions[4] = left + right / 2; - positions[5] = left - right; + const auto roll = positions.at(5); + positions[4] = left + roll / 2; + // positions[5] = left + right; js_msg.name = names; js_msg.position = positions; From a8f009a9fdc128212cb4addbcef06a8996206b88 Mon Sep 17 00:00:00 2001 From: Arthur Date: Thu, 21 Apr 2022 16:44:04 -0700 Subject: [PATCH 25/29] Added movit-visual-tools to project.json --- project.json | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/project.json b/project.json index 8dd30293..0bbb2578 100644 --- a/project.json +++ b/project.json @@ -34,6 +34,13 @@ "package": "ros-noetic-moveit", "provider": "apt" } + }, + { + "name": "moveit-visual-tools", + "build": { + "package": "ros-noetic-moveit-visual-tools", + "provider": "apt" + } } ] } From daed11c8a8af0184e794d34f957cf359435eb854 Mon Sep 17 00:00:00 2001 From: WR-BaseStation Date: Thu, 21 Apr 2022 20:15:49 -0500 Subject: [PATCH 26/29] Updated pitch math (turns out it was a direct reading all along). --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 2c9ebd67..fee60c27 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -177,9 +177,9 @@ void publishJointStates(const ros::TimerEvent &event){ } } - const auto left = positions.at(4); - const auto roll = positions.at(5); - positions[4] = left + roll / 2; + // const auto left = positions.at(4); + // const auto roll = positions.at(5); + // positions[4] = left + roll / 4; // positions[5] = left + right; js_msg.name = names; From acb8a8a627da3cb116cf425926a281fdbe3ef455 Mon Sep 17 00:00:00 2001 From: Ben Nowotny Date: Fri, 22 Apr 2022 11:49:08 -0500 Subject: [PATCH 27/29] Modifying speed handling in the arm and holding PID-enabled/`RUN_TO_POSITION` status. --- .../src/AbstractJoint.cpp | 8 +++++- .../src/AbstractJoint.hpp | 2 ++ .../src/ArmControlSystem.cpp | 10 +++----- src/wr_control_drive_arm/src/ArmMotor.cpp | 25 +++++++++++-------- src/wr_control_drive_arm/src/ArmMotor.hpp | 8 +++++- 5 files changed, 34 insertions(+), 19 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp index dab9a278..b677b443 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.cpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.cpp @@ -27,8 +27,14 @@ void AbstractJoint::configSetpoint(int degreeIndex, double position, double velo auto AbstractJoint::exectute() -> bool{ for(auto &motorHandle : motors){ - if (!motorHandle.motor->runToTarget(motorHandle.position, 0)) + if (!motorHandle.motor->runToTarget(motorHandle.position, motorHandle.velocity)); return false; } return true; +} + +void AbstractJoint::stopJoint(){ + for(auto &motorHandle : motors){ + motorHandle.motor->setPower(0.F); + } } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index 9bb61c00..522fe8a3 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -30,6 +30,8 @@ class AbstractJoint { auto exectute() -> bool; + void stopJoint(); + protected: vector motors; }; diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index fee60c27..3719ef77 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -32,6 +32,7 @@ constexpr float CLOCK_RATE = 50; constexpr double IK_WARN_RATE = 1.0/2; constexpr double JOINT_SAFETY_MAX_SPEED = 0.3; +constexpr double JOINT_SAFETY_HOLD_SPEED = 0.15; /** * @brief Nessage cache size of publisher @@ -107,9 +108,10 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server int currItr = 0; for(const auto &joint : joints){ for(int i = 0; i < joint->getDegreesOfFreedom(); i++){ - joint->configSetpoint(i, currTargetPosition.positions[currItr], JOINT_SAFETY_MAX_SPEED*currTargetPosition.velocities[currItr]/VELOCITY_MAX); + joint->configSetpoint(i, currTargetPosition.positions[currItr], VELOCITY_MAX == 0.F ? JOINT_SAFETY_HOLD_SPEED : JOINT_SAFETY_MAX_SPEED*currTargetPosition.velocities[currItr]/VELOCITY_MAX); currItr++; } + joint->exectute(); } // While the current position is not complete yet... @@ -152,12 +154,6 @@ void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server } //When all positions have been reached, set the current task as succeeded - - for(const auto &joint : joints){ - for(int i = 0; i < joint->getDegreesOfFreedom(); i++){ - joint->getMotor(i)->setPower(0.F); - } - } as->setSucceeded(); } diff --git a/src/wr_control_drive_arm/src/ArmMotor.cpp b/src/wr_control_drive_arm/src/ArmMotor.cpp index d081694d..57048943 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.cpp +++ b/src/wr_control_drive_arm/src/ArmMotor.cpp @@ -43,7 +43,7 @@ void ArmMotor::storeEncoderVals(const Std_UInt32 &msg){ void ArmMotor::redirectPowerOutput(const Std_Float64 &msg){ // Set the speed to be the contained data - this->setPower(msg->data); + if(this->getMotorState() == MotorState::RUN_TO_TARGET) this->setPower(static_cast(msg->data) * this->maxPower, MotorState::RUN_TO_TARGET); } void ArmMotor::storeStallStatus(const Std_Bool &msg) { @@ -66,6 +66,8 @@ ArmMotor::ArmMotor( controllerID{controllerID}, motorID{motorID}, currState{MotorState::STOP}, + power{0.F}, + maxPower{0.F}, encoderVal{static_cast(offset)} { // Check validity of WRoboclaw and motor IDs @@ -114,16 +116,21 @@ auto ArmMotor::getMotorState() const -> MotorState{ return this->currState; } -/// This method auto-publishes the speed command to the WRoboclaws void ArmMotor::setPower(float power){ + this->setPower(power, power == 0.F ? MotorState::STOP : MotorState::MOVING); +} + +/// This method auto-publishes the speed command to the WRoboclaws +void ArmMotor::setPower(float power, MotorState state){ // Check the bounds of the parameter if(abs(power) > 1) throw std::invalid_argument{std::string{"Power "} + std::to_string(power) + " is not on the interval [-1, 1]"}; // Set up and send the speed message + this->power = power; this->powerMsg.data = power * INT16_MAX; this->speedPub.publish(this->powerMsg); // Update the cur.nt motor state based on the power command input - this->currState = power == 0.F ? MotorState::STOP : MotorState::MOVING; + this->currState = state; } auto ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block) -> bool{ @@ -137,10 +144,11 @@ auto ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block) -> bo } else { begin = ros::Time::now(); } - + this->maxPower = power; // If we are not at our target... if(!this->hasReachedTarget(targetCounts)){ // Set the power in the correct direction and continue running to the target + this->currState = MotorState::RUN_TO_TARGET; std_msgs::Float64 setpointMsg; setpointMsg.data = ArmMotor::encToRad(targetCounts); this->targetPub.publish(setpointMsg); @@ -149,18 +157,15 @@ auto ArmMotor::runToTarget(uint32_t targetCounts, float power, bool block) -> bo // power = abs(power) * (corrMod(direction, ((long int)this->COUNTS_PER_ROTATION)) < corrMod(-direction, ((long int)this->COUNTS_PER_ROTATION)) ? 1 : -1); // this->setPower(power); - this->currState = MotorState::RUN_TO_TARGET; // Otherwise, stop the motor } else { - this->setPower(0.F); - this->currState = MotorState::STOP; + this->setPower(0.F, MotorState::RUN_TO_TARGET); } // If this is a blocking call... if(block){ // Wait until the motor has reached the target, then stop while(!this->hasReachedTarget(targetCounts)); - this->setPower(0.F); - this->currState = MotorState::STOP; + this->setPower(0.F, MotorState::RUN_TO_TARGET); } return true; } @@ -184,5 +189,5 @@ auto ArmMotor::getControllerID() const -> unsigned int{ } auto ArmMotor::getPower() const -> float{ - return ((float)this->powerMsg.data)/INT16_MAX; + return this->power; } \ No newline at end of file diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index 0962158c..c82676c2 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -42,7 +42,7 @@ class ArmMotor{ /// zero position of motor int64_t const ENCODER_OFFSET; /// The current state of the motor - MotorState currState; + std::atomic currState; /// The joint name of the current motor std::string motorName; /// The ID of the WRoboclaw controller @@ -69,6 +69,10 @@ class ArmMotor{ ros::Subscriber stallRead; /// The time when the motor began stalling ros::Time begin; + /// Motor power + float power; + /// Maximum absolute motor power in RUN_TO_POSITION mode. + std::atomic maxPower; /** * @brief A static conversion from radians to encoder counts @@ -107,6 +111,8 @@ class ArmMotor{ */ void storeStallStatus(const std_msgs::Bool::ConstPtr& msg); + void setPower(float power, MotorState state); + /** * @brief Performs Euclidean correct modulus between two inputs of the same type * From 57262190e9b0cc794d11e56e4e7dc0e7009bdc53 Mon Sep 17 00:00:00 2001 From: Arthur Date: Thu, 28 Apr 2022 17:20:33 -0700 Subject: [PATCH 28/29] added class headers --- src/wr_control_drive_arm/src/AbstractJoint.cpp | 3 ++- src/wr_control_drive_arm/src/AbstractJoint.hpp | 7 +++++++ src/wr_control_drive_arm/src/ArmMotor.hpp | 3 ++- src/wr_control_drive_arm/src/DifferentialJoint.cpp | 6 +++--- src/wr_control_drive_arm/src/DifferentialJoint.hpp | 7 +++++++ src/wr_control_drive_arm/src/SimpleJoint.cpp | 4 ++-- src/wr_control_drive_arm/src/SimpleJoint.hpp | 7 +++++++ 7 files changed, 30 insertions(+), 7 deletions(-) diff --git a/src/wr_control_drive_arm/src/AbstractJoint.cpp b/src/wr_control_drive_arm/src/AbstractJoint.cpp index b677b443..f425eeb6 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.cpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.cpp @@ -1,9 +1,10 @@ /** * @file AbstractJoint.cpp * @author Nichols Underwood - * @brief ablskjlfkejfs + * @brief Implementation of the AbstractJoint class * @date 2021-10-25 */ + #include "AbstractJoint.hpp" AbstractJoint::AbstractJoint(ros::NodeHandle &n, int numMotors){ diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index 522fe8a3..ca4d22ac 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -1,3 +1,10 @@ +/** + * @file AbstractJoint.hpp + * @author Nichols Underwood + * @brief Header file of the AbstractJoint class + * @date 2021-10-25 + */ + #ifndef ABSTRACT_JOINT_GUARD #define ABSTRACT_JOINT_GUARD diff --git a/src/wr_control_drive_arm/src/ArmMotor.hpp b/src/wr_control_drive_arm/src/ArmMotor.hpp index c82676c2..547c936d 100644 --- a/src/wr_control_drive_arm/src/ArmMotor.hpp +++ b/src/wr_control_drive_arm/src/ArmMotor.hpp @@ -1,9 +1,10 @@ /** * @file ArmMotor.hpp * @author Ben Nowotny - * @brief ablskjlfkejfs + * @brief Header file of the ArmMotor class * @date 2021-04-05 */ + #include #include "ros/ros.h" #include "std_msgs/UInt32.h" diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.cpp b/src/wr_control_drive_arm/src/DifferentialJoint.cpp index c71d7753..85c3c1c7 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.cpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.cpp @@ -1,7 +1,7 @@ /** - * @file AbstractJoint.hpp - * @author Nichols Underwood - * @brief ablskjlfkejfs + * @file DifferentialJoint.cpp + * @author Nicholas Underwood + * @brief Header file of the DifferentialJoint class * @date 2021-10-25 */ diff --git a/src/wr_control_drive_arm/src/DifferentialJoint.hpp b/src/wr_control_drive_arm/src/DifferentialJoint.hpp index d71fddd3..7bfaec4f 100644 --- a/src/wr_control_drive_arm/src/DifferentialJoint.hpp +++ b/src/wr_control_drive_arm/src/DifferentialJoint.hpp @@ -1,3 +1,10 @@ +/** + * @file DifferentialJoint.hpp + * @author Nichols Underwood + * @brief Header file of the DifferentialJoint class + * @date 2021-10-25 + */ + #include "AbstractJoint.hpp" class DifferentialJoint : public AbstractJoint { diff --git a/src/wr_control_drive_arm/src/SimpleJoint.cpp b/src/wr_control_drive_arm/src/SimpleJoint.cpp index 8617b2a0..d7ca2ee2 100644 --- a/src/wr_control_drive_arm/src/SimpleJoint.cpp +++ b/src/wr_control_drive_arm/src/SimpleJoint.cpp @@ -1,7 +1,7 @@ /** - * @file AbstractJoint.cpp + * @file SimpleJoint.cpp * @author Nichols Underwood - * @brief ablskjlfkejfs + * @brief Implementation of the SimpleJoint class * @date 2021-10-25 */ diff --git a/src/wr_control_drive_arm/src/SimpleJoint.hpp b/src/wr_control_drive_arm/src/SimpleJoint.hpp index 5819a60d..2c1bf76b 100644 --- a/src/wr_control_drive_arm/src/SimpleJoint.hpp +++ b/src/wr_control_drive_arm/src/SimpleJoint.hpp @@ -1,3 +1,10 @@ +/** + * @file SimpleJoint.hpp + * @author Nichols Underwood + * @brief Header file of the SimpleJoint class + * @date 2021-10-25 + */ + #include "AbstractJoint.hpp" using std::vector; From a9c3c96285e1f5f4381e677c95533777245de700 Mon Sep 17 00:00:00 2001 From: Arthur Date: Thu, 28 Apr 2022 17:51:49 -0700 Subject: [PATCH 29/29] Removed outdated comments --- src/wr_control_drive_arm/src/ArmControlSystem.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp index 3719ef77..e6136031 100644 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ b/src/wr_control_drive_arm/src/ArmControlSystem.cpp @@ -44,13 +44,6 @@ constexpr std::uint32_t MESSAGE_CACHE_SIZE = 1000; */ constexpr float TIMER_CALLBACK_DURATION = 1.0 / 50.0; -/** - * @brief Defines space for all ArmMotor references - */ -// constexpr int NUM_MOTORS = 6; -// std::array, NUM_MOTORS> motors; -// ArmMotor *motors[numMotors]; - /** * @brief Defines space for all Joint references */ @@ -173,11 +166,6 @@ void publishJointStates(const ros::TimerEvent &event){ } } - // const auto left = positions.at(4); - // const auto roll = positions.at(5); - // positions[4] = left + roll / 4; - // positions[5] = left + right; - js_msg.name = names; js_msg.position = positions; // Publish the Joint State message @@ -204,7 +192,6 @@ auto main(int argc, char** argv) ->int pn.getParam("encoder_parameters", encParams); // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node - // TODO: fix auto elbow = std::make_unique("elbow", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); auto forearmRoll = std::make_unique("forearm_roll", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); auto shoulder = std::make_unique("shoulder", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); @@ -214,7 +201,6 @@ auto main(int argc, char** argv) ->int std::cout << "init motors" << std::endl; // Initialize all Joints - // TODO: fix joints.at(0) = std::make_unique(std::move(elbow), n); joints.at(1) =std::make_unique(std::move(forearmRoll), n); joints.at(2) = std::make_unique(std::move(shoulder), n);