Joint state publisher application

.clang-format

@@ -0,0 +1,7 @@
+BasedOnStyle: LLVM
+IndentWidth: 4
+AccessModifierOffset: -4
+CommentPragmas: 'NOLINT'
+PackConstructorInitializers: CurrentLine
+AlignOperands: AlignAfterOperator
+AlignTrailingComments: true

.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'

.clangd

@@ -0,0 +1,2 @@
+Diagnostics:
+    UnusedIncludes: Strict

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

src/wr_control_drive_arm/launch/actionserver.launch

@@ -0,0 +1,9 @@
+<!--Launches the action server and PID nodes-->
+<launch>
+    <group ns="control">
+        <node name="WResponseControl_ArmMotors" pkg="wresponsecontrol" type="WResponseControl">
+            <rosparam file="$(find wr_control_drive_arm)/config/arm_motor_PID.yaml"/>
+        </node>
+        <node name="ArmControlActionServer" pkg="wr_control_drive_arm" type="ArmControlActionServer" output="screen"/>
+    </group>
+</launch>

src/wr_control_drive_arm/launch/jointstatepublisher.launch

@@ -0,0 +1,9 @@
+<!--Launches the joint state publisher node-->
+<launch>
+    <group ns="control">
+        <node name="JointStatePublisher" pkg="wr_control_drive_arm" type="JointStatePublisher" output="screen">
+            <rosparam file="$(find wr_control_drive_arm)/config/encoder_parameters_real.yaml" if="$(eval env('WROVER_HW') == 'REAL')"/>
+            <rosparam file="$(find wr_control_drive_arm)/config/encoder_parameters_mock.yaml" if="$(eval env('WROVER_HW') == 'MOCK')"/>
+        </node>
+    </group>
+</launch>

src/wr_control_drive_arm/launch/std.launch

@@ -1,12 +1,5 @@
+<!--Launches the action server and the joint state publisher-->
 <launch>
-    <group ns="control">
-        <node name="WResponseControl_ArmMotors" pkg="wresponsecontrol" type="WResponseControl">
-            <rosparam file="$(find wr_control_drive_arm)/config/arm_motor_PID_real.yaml" if="$(eval env('WROVER_HW') == 'REAL')"/>
-            <rosparam file="$(find wr_control_drive_arm)/config/arm_motor_PID_mock.yaml" if="$(eval env('WROVER_HW') == 'MOCK')"/>
-        </node>
-        <node name="ArmControlSystem" pkg="wr_control_drive_arm" type="ArmControlSystem" output="screen">
-            <rosparam file="$(find wr_control_drive_arm)/config/encoder_parameters_real.yaml" if="$(eval env('WROVER_HW') == 'REAL')"/>
-            <rosparam file="$(find wr_control_drive_arm)/config/encoder_parameters_mock.yaml" if="$(eval env('WROVER_HW') == 'MOCK')"/>
-        </node>
-    </group>
+    <include file="$(find wr_control_drive_arm)/launch/actionserver.launch" />
+    <include file="$(find wr_control_drive_arm)/launch/jointstatepublisher.launch" />
 </launch>

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<MotorHandler> motors;
 };

src/wr_control_drive_arm/src/ArmControlSystem.cpp → 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,42 +218,16 @@ 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<ArmMotor>("elbowPitch_joint", 1, 0, static_cast<int>(encParams[0]["counts_per_rotation"]), static_cast<int>(encParams[0]["offset"]), n);
-  auto elbowRoll_joint = std::make_unique<ArmMotor>("elbowRoll_joint", 1, 1, static_cast<int>(encParams[1]["counts_per_rotation"]), static_cast<int>(encParams[1]["offset"]), n);
-  auto shoulder_joint = std::make_unique<ArmMotor>("shoulder_joint", 0, 1, static_cast<int>(encParams[2]["counts_per_rotation"]), static_cast<int>(encParams[2]["offset"]), n);
-  auto turntable_joint = std::make_unique<ArmMotor>("turntable_joint", 0, 0, static_cast<int>(encParams[3]["counts_per_rotation"]), static_cast<int>(encParams[3]["offset"]), n);
-  auto wristPitch_joint = std::make_unique<ArmMotor>("wristPitch_joint", 2, 0, static_cast<int>(encParams[4]["counts_per_rotation"]), static_cast<int>(encParams[4]["offset"]), n);
-  auto wristRoll_link = std::make_unique<ArmMotor>("wristRoll_link", 2, 1, static_cast<int>(encParams[5]["counts_per_rotation"]), static_cast<int>(encParams[5]["offset"]), n);
-  std::cout << "init motors" << std::endl;
-
-  // Initialize all Joints
-  joints.at(3) = std::make_unique<SimpleJoint>(std::move(turntable_joint), n);
-  joints.at(2) = std::make_unique<SimpleJoint>(std::move(shoulder_joint), n);
-  joints.at(0) = std::make_unique<SimpleJoint>(std::move(elbowPitch_joint), n);
-  joints.at(1) = std::make_unique<SimpleJoint>(std::move(elbowRoll_joint), n);
-  joints.at(4) = std::make_unique<DifferentialJoint>(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<sensor_msgs::JointState>("/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<boost::function<bool(std_srvs::Trigger::Request&, std_srvs::Trigger::Response&)>>(
       [](std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)->bool{

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 <control_msgs/FollowJointTrajectoryAction.h>
+#include <actionlib/server/simple_action_server.h>
+#include <sensor_msgs/JointState.h>
+#include <std_msgs/Float64.h>
+#include <std_msgs/Empty.h>
+#include <algorithm>
+#include <csignal>
+#include <string>
+#include <array>
+#include <memory>
+#include <std_srvs/Trigger.h>
+#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<std::unique_ptr<AbstractJoint>, NUM_JOINTS> joints;
+
+/**
+ * @brief The joint state publisher for MoveIt
+ */
+ros::Publisher jointStatePublisher;
+
+void publishJointStates(const ros::TimerEvent &event){
+  std::vector<std::string> names;
+  std::vector<double> 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<ArmMotor>("elbowPitch_joint", 1, 0, static_cast<int>(encParams[0]["counts_per_rotation"]), static_cast<int>(encParams[0]["offset"]), n);
+    auto elbowRoll_joint = std::make_unique<ArmMotor>("elbowRoll_joint", 1, 1, static_cast<int>(encParams[1]["counts_per_rotation"]), static_cast<int>(encParams[1]["offset"]), n);
+    auto shoulder_joint = std::make_unique<ArmMotor>("shoulder_joint", 0, 1, static_cast<int>(encParams[2]["counts_per_rotation"]), static_cast<int>(encParams[2]["offset"]), n);
+    auto turntable_joint = std::make_unique<ArmMotor>("turntable_joint", 0, 0, static_cast<int>(encParams[3]["counts_per_rotation"]), static_cast<int>(encParams[3]["offset"]), n);
+    auto wristPitch_joint = std::make_unique<ArmMotor>("wristPitch_joint", 2, 0, static_cast<int>(encParams[4]["counts_per_rotation"]), static_cast<int>(encParams[4]["offset"]), n);
+    auto wristRoll_link = std::make_unique<ArmMotor>("wristRoll_link", 2, 1, static_cast<int>(encParams[5]["counts_per_rotation"]), static_cast<int>(encParams[5]["offset"]), n);
+    std::cout << "init motors" << std::endl;
+
+    // Initialize all Joints
+    joints.at(0) = std::make_unique<SimpleJoint>(std::move(elbowPitch_joint), n);
+    joints.at(1) = std::make_unique<SimpleJoint>(std::move(elbowRoll_joint), n);
+    joints.at(2) = std::make_unique<SimpleJoint>(std::move(shoulder_joint), n);
+    joints.at(3) = std::make_unique<SimpleJoint>(std::move(turntable_joint), n);
+    joints.at(4) = std::make_unique<DifferentialJoint>(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<sensor_msgs::JointState>("/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;
+}