From 03839bfc4c808b5b6dee348b94a981de04a3346b Mon Sep 17 00:00:00 2001 From: Jack Zautner Date: Mon, 5 Dec 2022 19:48:40 -0600 Subject: [PATCH 1/3] Separated action server and joint state publisher into separate applications for arm control system --- .clang-format | 7 + .clang-tidy | 10 + .clangd | 2 + src/wr_control_drive_arm/CMakeLists.txt | 6 +- .../launch/actionserver.launch | 9 + .../launch/jointstatepublisher.launch | 9 + src/wr_control_drive_arm/launch/std.launch | 12 +- .../src/AbstractJoint.hpp | 10 + .../src/ArmControlActionServer.cpp | 248 ++++++++++++++++++ .../src/JointStatePublisher.cpp | 114 ++++++++ 10 files changed, 416 insertions(+), 11 deletions(-) create mode 100644 .clang-format create mode 100644 .clang-tidy create mode 100644 .clangd create mode 100644 src/wr_control_drive_arm/launch/actionserver.launch create mode 100644 src/wr_control_drive_arm/launch/jointstatepublisher.launch create mode 100644 src/wr_control_drive_arm/src/ArmControlActionServer.cpp create mode 100644 src/wr_control_drive_arm/src/JointStatePublisher.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..c6ba5550 --- /dev/null +++ b/.clang-format @@ -0,0 +1,7 @@ +BasedOnStyle: LLVM +IndentWidth: 4 +AccessModifierOffset: -4 +CommentPragmas: 'NOLINT' +PackConstructorInitializers: CurrentLine +AlignOperands: AlignAfterOperator +AlignTrailingComments: true \ No newline at end of file diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 00000000..e9f4e763 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,10 @@ +Checks: "bugprone-*,performance-*,readability-*,modernize-*,hicpp-*,cppcoreguidelines-*,clang-analyzer-*,-readability-braces-around-statements,-hicpp-braces-around-statements,-cppcoreguidelines-non-private-member-variables-in-classes" +WarningsAsErrors: '' +HeaderFilterRegex: '' +CheckOptions: + - key: readability-magic-numbers.IgnoredIntegerValues + value: '1;' + - key: hicpp-signed-bitwise.IgnorePositiveIntegerLiterals + value: 'true' + - key: readability-function-cognitive-complexity.IgnoreMacros + value: 'true' \ No newline at end of file diff --git a/.clangd b/.clangd new file mode 100644 index 00000000..538f880e --- /dev/null +++ b/.clangd @@ -0,0 +1,2 @@ +Diagnostics: + UnusedIncludes: Strict \ No newline at end of file diff --git a/src/wr_control_drive_arm/CMakeLists.txt b/src/wr_control_drive_arm/CMakeLists.txt index c9d43c92..19ef7fde 100644 --- a/src/wr_control_drive_arm/CMakeLists.txt +++ b/src/wr_control_drive_arm/CMakeLists.txt @@ -139,8 +139,10 @@ include_directories( ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/wr_control_drive_arm_node.cpp) -add_executable(ArmControlSystem src/ArmControlSystem.cpp) -target_link_libraries(ArmControlSystem ${catkin_LIBRARIES} ${PROJECT_NAME}) +add_executable(ArmControlActionServer src/ArmControlActionServer.cpp) +target_link_libraries(ArmControlActionServer ${catkin_LIBRARIES} ${PROJECT_NAME}) +add_executable(JointStatePublisher src/JointStatePublisher.cpp) +target_link_libraries(JointStatePublisher ${catkin_LIBRARIES} ${PROJECT_NAME}) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/src/wr_control_drive_arm/launch/actionserver.launch b/src/wr_control_drive_arm/launch/actionserver.launch new file mode 100644 index 00000000..4ff64772 --- /dev/null +++ b/src/wr_control_drive_arm/launch/actionserver.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/src/wr_control_drive_arm/launch/jointstatepublisher.launch b/src/wr_control_drive_arm/launch/jointstatepublisher.launch new file mode 100644 index 00000000..eb85aac1 --- /dev/null +++ b/src/wr_control_drive_arm/launch/jointstatepublisher.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/src/wr_control_drive_arm/launch/std.launch b/src/wr_control_drive_arm/launch/std.launch index e9a2fb2f..96c2e726 100644 --- a/src/wr_control_drive_arm/launch/std.launch +++ b/src/wr_control_drive_arm/launch/std.launch @@ -1,11 +1,5 @@ + - - - - - - - - - + + diff --git a/src/wr_control_drive_arm/src/AbstractJoint.hpp b/src/wr_control_drive_arm/src/AbstractJoint.hpp index cf19f069..b395aa61 100644 --- a/src/wr_control_drive_arm/src/AbstractJoint.hpp +++ b/src/wr_control_drive_arm/src/AbstractJoint.hpp @@ -39,6 +39,16 @@ class AbstractJoint { void stopJoint(); + virtual ~AbstractJoint() = default; + + AbstractJoint(const AbstractJoint&) = delete; + + auto operator=(const AbstractJoint&) -> AbstractJoint& = delete; + + AbstractJoint(AbstractJoint&&) = delete; + + auto operator=(AbstractJoint&&) -> AbstractJoint& = delete; + protected: vector motors; }; diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp new file mode 100644 index 00000000..0e67d57b --- /dev/null +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -0,0 +1,248 @@ +/** + * @file ArmControlActionServer.cpp + * @author Ben Nowotny + * @brief The exeutable file to run the Arm Control Action Server + * @date 2021-04-05 + */ +#include "XmlRpcValue.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SimpleJoint.hpp" +#include "DifferentialJoint.hpp" +#include "ros/console.h" + +using XmlRpc::XmlRpcValue; + +/** + * @brief Refresh rate of ros::Rate + */ +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 + */ +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 Joint references + */ +constexpr int NUM_JOINTS = 5; +std::array, NUM_JOINTS> joints; +// AbstractJoint *joints[numJoints]; +/** + * @brief The Joint State Publisher for MoveIt + */ +ros::Publisher jointStatePublisher; +/** + * @brief Simplify the SimpleActionServer reference name + */ +typedef actionlib::SimpleActionServer Server; +/** + * @brief The service server for enabling IK + */ +ros::ServiceServer enableServiceServer; +/** + * @brief The status of IK program + */ +std::atomic_bool IKEnabled{true}; +/** + * @brief The service client for disabling IK + */ +ros::ServiceClient enableServiceClient; + +/** + * @brief Perform the given action as interpreted as moving the arm joints to specified positions + * + * @param goal The goal state given + * @param as The Action Server this is occuring on + */ +void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { + if (!IKEnabled) { + as->setAborted(); + 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; + } + + int currPoint = 1; + + std::cout << "start exec: " << goal->trajectory.points.size() << std::endl; + // For each point in the trajectory execution sequence... + for(const auto& name : goal->trajectory.joint_names){ + std::cout << name << "\t"; + } + std::cout << std::endl; + for(const auto &currTargetPosition : goal->trajectory.points){ + for(double pos : currTargetPosition.positions){ + std::cout << std::round(pos*100)/100 << "\t"; + } + std::cout << std::endl; + } + 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}; + + const double VELOCITY_MAX = abs(*std::max_element( + currTargetPosition.velocities.begin(), + currTargetPosition.velocities.end(), + [](double a, double b) -> bool{ return abs(a)trajectory.points.size() << std::endl; + currPoint++; + for(const auto &joint : joints){ + for(int i = 0; i < joint->getDegreesOfFreedom(); i++){ + double velocity = VELOCITY_MAX == 0.F ? JOINT_SAFETY_HOLD_SPEED : currTargetPosition.velocities[currItr]/VELOCITY_MAX; + // std::cout << "config setpoint: " << currTargetPosition.positions[currItr] << ":" << velocity << std::endl; + joint->configSetpoint(i, currTargetPosition.positions[currItr], velocity); + currItr++; + } + joint->exectute(); + } + + // 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 + + // For each joint specified in the currTargetPosition... + for(const auto &joint : joints){ + + for(int k = 0; k < joint->getDegreesOfFreedom(); k++){ + // if (joint->getMotor(k)->getMotorState() == MotorState::MOVING) { + // std::cout << "Moving" << std::endl; + // } else if (joint->getMotor(k)->getMotorState() == MotorState::RUN_TO_TARGET) { + // std::cout << "Run to target" << std::endl; + // } else if (joint->getMotor(k)->getMotorState() == MotorState::STALLING) { + // std::cout << "Stalling" << std::endl; + // } else if (joint->getMotor(k)->getMotorState() == MotorState::STOP) { + // std::cout << "Stop" << std::endl; + // } else { + // std::cout << "Error" << std::endl; + // } + + if (joint->getMotor(k)->getMotorState() == MotorState::STALLING) { + std::cout << "ACS stall detected" << std::endl; + IKEnabled = false; + std_srvs::Trigger srv; + if (enableServiceClient.call(srv)) { + 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"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) + } + } else { + hasPositionFinished &= joint->getMotor(k)->getMotorState() == MotorState::STOP; + } + // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power + // std::cout<getEncoderCounts()<exectute(); + + // if (!joint->exectute()) { + // IKEnabled = false; + // std_srvs::Trigger srv; + // if (enableServiceClient.call(srv)) { + // 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"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) + // } + // return; + // } + } + // Sleep until the next update cycle + loop.sleep(); + } + } + + //When all positions have been reached, set the current task as succeeded + + as->setSucceeded(); +} + +/** + * @brief publishes the arm's position + */ +void publishJointStates(const ros::TimerEvent &event){ + std::vector names; + std::vector positions; + sensor_msgs::JointState js_msg; + + 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()); + } + } + + js_msg.name = names; + js_msg.position = positions; + js_msg.header.stamp = ros::Time::now(); + // Publish the Joint State message + jointStatePublisher.publish(js_msg); +} + +/** + * @brief The main executable method of the node. Starts the ROS node and the Action Server for processing Arm Control commands + * + * @param argc The number of program arguments + * @param argv The given program arguments + * @return int The status code on exiting the program + */ +auto main(int argc, char** argv) ->int +{ + std::cout << "start main" << std::endl; + // Initialize the current node as ArmControlSystem + ros::init(argc, argv, "ArmControlActionServer"); + // Create the NodeHandle to the current ROS node + ros::NodeHandle n; + + // Initialize the Action Server + Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); + // Start the Action Server + server.start(); + std::cout << "server started" << std::endl; + + 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 = static_cast(true); + return true; + } + )); + + enableServiceClient = n.serviceClient("PLACEHOLDER_NAME"); + + std::cout << "entering ROS spin..." << std::endl; + // ROS spin for communication with other nodes + ros::spin(); + // Return 0 on exit (successful exit) + return 0; +} diff --git a/src/wr_control_drive_arm/src/JointStatePublisher.cpp b/src/wr_control_drive_arm/src/JointStatePublisher.cpp new file mode 100644 index 00000000..b6d47251 --- /dev/null +++ b/src/wr_control_drive_arm/src/JointStatePublisher.cpp @@ -0,0 +1,114 @@ +/** + * @file JointStatePublisher.cpp + * @author Ben Nowotny + * @author Jack Zautner + * @brief The executable file to run the joint state publisher + * @date 2022-12-05 + */ +#include "XmlRpcValue.h" + +#include "ros/ros.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "SimpleJoint.hpp" +#include "DifferentialJoint.hpp" +#include "ros/console.h" + +using XmlRpc::XmlRpcValue; + +/** + * @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 Joint references + */ +constexpr int NUM_JOINTS = 5; +std::array, NUM_JOINTS> joints; + +/** + * @brief The joint state publisher for MoveIt + */ +ros::Publisher jointStatePublisher; + +void publishJointStates(const ros::TimerEvent &event){ + std::vector names; + std::vector positions; + sensor_msgs::JointState js_msg; + + 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()); + } + } + + js_msg.name = names; + js_msg.position = positions; + js_msg.header.stamp = ros::Time::now(); + // Publish the Joint State message + jointStatePublisher.publish(js_msg); +} + +/** + * @brief The main executable method of the node. Starts the ROS node + * + * @param argc The number of program arguments + * @param argv The given program arguments + * @return int The status code on exiting the program + */ +auto main(int argc, char** argv) -> int { + + std::cout << "start main" << std::endl; + // Initialize the current node as JointStatePublisherApplication + ros::init(argc, argv, "JointStatePublisher"); + // Create the NodeHandle to the current ROS node + ros::NodeHandle n; + ros::NodeHandle pn{"~"}; + + XmlRpcValue encParams; + pn.getParam("encoder_parameters", encParams); + + // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node + auto elbowPitch_joint = std::make_unique("elbowPitch_joint", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); + auto elbowRoll_joint = std::make_unique("elbowRoll_joint", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); + auto shoulder_joint = std::make_unique("shoulder_joint", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); + auto turntable_joint = std::make_unique("turntable_joint", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n); + auto wristPitch_joint = std::make_unique("wristPitch_joint", 2, 0, static_cast(encParams[4]["counts_per_rotation"]), static_cast(encParams[4]["offset"]), n); + auto wristRoll_link = std::make_unique("wristRoll_link", 2, 1, static_cast(encParams[5]["counts_per_rotation"]), static_cast(encParams[5]["offset"]), n); + std::cout << "init motors" << std::endl; + + // Initialize all Joints + joints.at(0) = std::make_unique(std::move(elbowPitch_joint), n); + joints.at(1) = std::make_unique(std::move(elbowRoll_joint), n); + joints.at(2) = std::make_unique(std::move(shoulder_joint), n); + joints.at(3) = std::make_unique(std::move(turntable_joint), n); + joints.at(4) = std::make_unique(std::move(wristPitch_joint), std::move(wristRoll_link), n, "/control/arm/5/pitch", "/control/arm/5/roll", "/control/arm/20/", "/control/arm/21/"); + std::cout << "init joints" << std::endl; + + // Initialize the Joint State Data Publisher + jointStatePublisher = n.advertise("/joint_states", MESSAGE_CACHE_SIZE); + + // Timer that will call publishJointStates periodically + ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publishJointStates); + + // Enter ROS spin + ros::spin(); + + return 0; +} \ No newline at end of file From 1fc442f3557c6b8c1654251c04f52af97e004894 Mon Sep 17 00:00:00 2001 From: Jack Zautner Date: Mon, 5 Dec 2022 19:51:18 -0600 Subject: [PATCH 2/3] Deleted old ArmControlSystem file --- .../src/ArmControlSystem.cpp | 274 ------------------ 1 file changed, 274 deletions(-) delete mode 100644 src/wr_control_drive_arm/src/ArmControlSystem.cpp diff --git a/src/wr_control_drive_arm/src/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlSystem.cpp deleted file mode 100644 index a7681e89..00000000 --- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp +++ /dev/null @@ -1,274 +0,0 @@ -/** - * @file ArmControlSystem.cpp - * @author Ben Nowotny - * @brief The exeutable file to run the Arm Control Action Server - * @date 2021-04-05 - */ -#include "XmlRpcValue.h" - -#include "ros/ros.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "SimpleJoint.hpp" -#include "DifferentialJoint.hpp" -#include "ros/console.h" - -using XmlRpc::XmlRpcValue; - -/** - * @brief Refresh rate of ros::Rate - */ -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 - */ -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 Joint references - */ -constexpr int NUM_JOINTS = 5; -std::array, NUM_JOINTS> joints; -// AbstractJoint *joints[numJoints]; -/** - * @brief The Joint State Publisher for MoveIt - */ -ros::Publisher jointStatePublisher; -/** - * @brief Simplify the SimpleActionServer reference name - */ -typedef actionlib::SimpleActionServer Server; -/** - * @brief The service server for enabling IK - */ -ros::ServiceServer enableServiceServer; -/** - * @brief The status of IK program - */ -std::atomic_bool IKEnabled{true}; -/** - * @brief The service client for disabling IK - */ -ros::ServiceClient enableServiceClient; - -/** - * @brief Perform the given action as interpreted as moving the arm joints to specified positions - * - * @param goal The goal state given - * @param as The Action Server this is occuring on - */ -void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as) { - if (!IKEnabled) { - as->setAborted(); - 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; - } - - int currPoint = 1; - - std::cout << "start exec: " << goal->trajectory.points.size() << std::endl; - // For each point in the trajectory execution sequence... - for(const auto& name : goal->trajectory.joint_names){ - std::cout << name << "\t"; - } - std::cout << std::endl; - for(const auto &currTargetPosition : goal->trajectory.points){ - for(double pos : currTargetPosition.positions){ - std::cout << std::round(pos*100)/100 << "\t"; - } - std::cout << std::endl; - } - 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}; - - const double VELOCITY_MAX = abs(*std::max_element( - currTargetPosition.velocities.begin(), - currTargetPosition.velocities.end(), - [](double a, double b) -> bool{ return abs(a)trajectory.points.size() << std::endl; - currPoint++; - for(const auto &joint : joints){ - for(int i = 0; i < joint->getDegreesOfFreedom(); i++){ - double velocity = VELOCITY_MAX == 0.F ? JOINT_SAFETY_HOLD_SPEED : currTargetPosition.velocities[currItr]/VELOCITY_MAX; - // std::cout << "config setpoint: " << currTargetPosition.positions[currItr] << ":" << velocity << std::endl; - joint->configSetpoint(i, currTargetPosition.positions[currItr], velocity); - currItr++; - } - joint->exectute(); - } - - // 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 - - // For each joint specified in the currTargetPosition... - for(const auto &joint : joints){ - - for(int k = 0; k < joint->getDegreesOfFreedom(); k++){ - // if (joint->getMotor(k)->getMotorState() == MotorState::MOVING) { - // std::cout << "Moving" << std::endl; - // } else if (joint->getMotor(k)->getMotorState() == MotorState::RUN_TO_TARGET) { - // std::cout << "Run to target" << std::endl; - // } else if (joint->getMotor(k)->getMotorState() == MotorState::STALLING) { - // std::cout << "Stalling" << std::endl; - // } else if (joint->getMotor(k)->getMotorState() == MotorState::STOP) { - // std::cout << "Stop" << std::endl; - // } else { - // std::cout << "Error" << std::endl; - // } - - if (joint->getMotor(k)->getMotorState() == MotorState::STALLING) { - std::cout << "ACS stall detected" << std::endl; - IKEnabled = false; - std_srvs::Trigger srv; - if (enableServiceClient.call(srv)) { - 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"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) - } - } else { - hasPositionFinished &= joint->getMotor(k)->getMotorState() == MotorState::STOP; - } - // DEBUGGING OUTPUT: Print each motor's name, radian position, encoder position, and power - // std::cout<getEncoderCounts()<exectute(); - - // if (!joint->exectute()) { - // IKEnabled = false; - // std_srvs::Trigger srv; - // if (enableServiceClient.call(srv)) { - // 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"); // NOLINT(cppcoreguidelines-pro-bounds-array-to-pointer-decay, cppcoreguidelines-pro-type-vararg) - // } - // return; - // } - } - // Sleep until the next update cycle - loop.sleep(); - } - } - - //When all positions have been reached, set the current task as succeeded - - as->setSucceeded(); -} - -/** - * @brief publishes the arm's position - */ -void publishJointStates(const ros::TimerEvent &event){ - std::vector names; - std::vector positions; - sensor_msgs::JointState js_msg; - - 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()); - } - } - - js_msg.name = names; - js_msg.position = positions; - js_msg.header.stamp = ros::Time::now(); - // Publish the Joint State message - jointStatePublisher.publish(js_msg); -} - -/** - * @brief The main executable method of the node. Starts the ROS node and the Action Server for processing Arm Control commands - * - * @param argc The number of program arguments - * @param argv The given program arguments - * @return int The status code on exiting the program - */ -auto main(int argc, char** argv) ->int -{ - std::cout << "start main" << std::endl; - // Initialize the current node as ArmControlSystem - ros::init(argc, argv, "ArmControlSystem"); - // Create the NodeHandle to the current ROS node - ros::NodeHandle n; - ros::NodeHandle pn{"~"}; - - XmlRpcValue encParams; - pn.getParam("encoder_parameters", encParams); - - // Initialize all motors with their MoveIt name, WRoboclaw initialization, and reference to the current node - auto elbowPitch_joint = std::make_unique("elbowPitch_joint", 1, 0, static_cast(encParams[0]["counts_per_rotation"]), static_cast(encParams[0]["offset"]), n); - auto elbowRoll_joint = std::make_unique("elbowRoll_joint", 1, 1, static_cast(encParams[1]["counts_per_rotation"]), static_cast(encParams[1]["offset"]), n); - auto shoulder_joint = std::make_unique("shoulder_joint", 0, 1, static_cast(encParams[2]["counts_per_rotation"]), static_cast(encParams[2]["offset"]), n); - auto turntable_joint = std::make_unique("turntable_joint", 0, 0, static_cast(encParams[3]["counts_per_rotation"]), static_cast(encParams[3]["offset"]), n); - auto wristPitch_joint = std::make_unique("wristPitch_joint", 2, 0, static_cast(encParams[4]["counts_per_rotation"]), static_cast(encParams[4]["offset"]), n); - auto wristRoll_link = std::make_unique("wristRoll_link", 2, 1, static_cast(encParams[5]["counts_per_rotation"]), static_cast(encParams[5]["offset"]), n); - std::cout << "init motors" << std::endl; - - // Initialize all Joints - joints.at(3) = std::make_unique(std::move(turntable_joint), n); - joints.at(2) = std::make_unique(std::move(shoulder_joint), n); - joints.at(0) = std::make_unique(std::move(elbowPitch_joint), n); - joints.at(1) = std::make_unique(std::move(elbowRoll_joint), n); - joints.at(4) = std::make_unique(std::move(wristPitch_joint), std::move(wristRoll_link), n, "/control/arm/5/pitch", "/control/arm/5/roll", "/control/arm/20/", "/control/arm/21/"); - std::cout << "init joints" << std::endl; - - // Initialize the Joint State Data Publisher - jointStatePublisher = n.advertise("/joint_states", MESSAGE_CACHE_SIZE); - - // Initialize the Action Server - Server server(n, "/arm_controller/follow_joint_trajectory", boost::bind(&execute, _1, &server), false); - // Start the Action Server - server.start(); - std::cout << "server started" << std::endl; - - ros::Timer timer = n.createTimer(ros::Duration(TIMER_CALLBACK_DURATION), publishJointStates); - - 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 = static_cast(true); - return true; - } - )); - - enableServiceClient = n.serviceClient("PLACEHOLDER_NAME"); - - std::cout << "entering ROS spin..." << std::endl; - // ROS spin for communication with other nodes - ros::spin(); - // Return 0 on exit (successful exit) - return 0; -} From 9511729325c4ca8c1cf4a4da8c5fe4c8ca1faf55 Mon Sep 17 00:00:00 2001 From: Jack Zautner Date: Wed, 7 Dec 2022 17:55:37 -0600 Subject: [PATCH 3/3] Fixed date in header --- src/wr_control_drive_arm/src/ArmControlActionServer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp index 0e67d57b..a178f157 100644 --- a/src/wr_control_drive_arm/src/ArmControlActionServer.cpp +++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp @@ -2,7 +2,7 @@ * @file ArmControlActionServer.cpp * @author Ben Nowotny * @brief The exeutable file to run the Arm Control Action Server - * @date 2021-04-05 + * @date 2022-12-05 */ #include "XmlRpcValue.h"