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 8d5d412f..96c2e726 100644
--- a/src/wr_control_drive_arm/launch/std.launch
+++ b/src/wr_control_drive_arm/launch/std.launch
@@ -1,12 +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/ArmControlSystem.cpp b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp
similarity index 79%
rename from src/wr_control_drive_arm/src/ArmControlSystem.cpp
rename to src/wr_control_drive_arm/src/ArmControlActionServer.cpp
index a7681e89..a178f157 100644
--- a/src/wr_control_drive_arm/src/ArmControlSystem.cpp
+++ b/src/wr_control_drive_arm/src/ArmControlActionServer.cpp
@@ -1,8 +1,8 @@
/**
- * @file ArmControlSystem.cpp
+ * @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"
@@ -218,33 +218,9 @@ auto main(int argc, char** argv) ->int
{
std::cout << "start main" << std::endl;
// Initialize the current node as ArmControlSystem
- ros::init(argc, argv, "ArmControlSystem");
+ ros::init(argc, argv, "ArmControlActionServer");
// 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);
@@ -252,8 +228,6 @@ 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), publishJointStates);
-
enableServiceServer = n.advertiseService("start_IK",
static_cast>(
[](std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)->bool{
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