created forward jacobian determinant node

Author: jeferal

src/pr_modelling/CMakeLists.txt

@@ -195,7 +195,26 @@ ament_target_dependencies(ang_ots
   "pr_lib")
 
 rclcpp_components_register_nodes(ang_ots "pr_modelling::AngOTS")
-set(node_plugins "${node_plugins}pr_modelling::StatePublisher;$<TARGET_FILE:ang_ots>\n")
+set(node_plugins "${node_plugins}pr_modelling::AngOTS;$<TARGET_FILE:ang_ots>\n")
+
+
+### Forward Jacobian node ###
+
+add_library(for_jac SHARED
+  src/for_jac.cpp)
+
+target_compile_definitions(for_jac
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+
+ament_target_dependencies(for_jac
+  "rclcpp"
+  "rclcpp_components"
+  "pr_msgs"
+  "pr_lib")
+
+rclcpp_components_register_nodes(for_jac "pr_modelling::ForwardJacobian")
+set(node_plugins "${node_plugins}pr_modelling::ForwardJacobian;$<TARGET_FILE:for_jac>\n")
+
 
 
 ### Installation ###
@@ -209,6 +228,7 @@ install(TARGETS
   q_grav
   state_publisher
   ang_ots
+  for_jac
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)

src/pr_modelling/include/pr_modelling/for_jac.hpp

@@ -0,0 +1,33 @@
+#ifndef PR_MODELLING__FOR_JAC_HPP_
+#define PR_MODELLING__FOR_JAC_HPP_
+
+#include <vector>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "pr_msgs/msg/pr_array_h.hpp"
+#include "pr_msgs/msg/pr_float_h.hpp"
+
+#include "pr_lib/pr_model.hpp"
+
+namespace pr_modelling
+{
+    class ForwardJacobian : public rclcpp::Node
+    {
+        public:
+            //PR_MODELLING_PUBLIC
+            explicit ForwardJacobian(const rclcpp::NodeOptions & options);
+
+        protected:
+            void topic_callback(const pr_msgs::msg::PRArrayH::SharedPtr x_msg);
+
+        private:
+            rclcpp::Subscription<pr_msgs::msg::PRArrayH>::SharedPtr subscription_;
+            rclcpp::Publisher<pr_msgs::msg::PRFloatH>::SharedPtr publisher_;
+
+            Eigen::Matrix<double, 4, 4> ForJac = Eigen::Matrix<double, 4, 4>::Zero();
+            std::vector<double> robot_params;
+    };
+}
+
+#endif // PR_MODELLING__IND_JAC_HPP_

src/pr_modelling/src/for_jac.cpp

@@ -0,0 +1,58 @@
+#include "pr_modelling/for_jac.hpp"
+
+#include <chrono>
+#include <memory>
+#include <utility>
+#include <array>
+#include <vector>
+
+#include "pr_lib/pr_model.hpp"
+
+using std::placeholders::_1;
+
+namespace pr_modelling
+{
+    /**** FORWARD JACOBIAN COMPONENT ****/
+    ForwardJacobian::ForwardJacobian(const rclcpp::NodeOptions & options)
+    : Node("for_jac", options)
+    {
+        //Parameter declaration
+        this->declare_parameter<std::vector<double>>(
+            "robot_config_params", 
+            {0.4, 0.4, 0.4, 0.15, 90*(M_PI/180), 45*(M_PI/180), 0.3, 0.3, 0.3, 50*(M_PI/180), 90*(M_PI/180)});
+        
+        this->get_parameter("robot_config_params", robot_params);
+
+        subscription_ = this->create_subscription<pr_msgs::msg::PRArrayH>(
+            "x_coord", 
+            10, 
+            std::bind(&ForwardJacobian::topic_callback, this, _1));
+
+        publisher_ = this->create_publisher<pr_msgs::msg::PRFloatH>(
+            "for_jac_det", 
+            1);
+    }
+
+    void ForwardJacobian::topic_callback(const pr_msgs::msg::PRArrayH::SharedPtr x_msg)
+    {
+        PRModel::ForwardJacobian(ForJac, x_msg->data, robot_params);
+
+        double for_jac_det = ForJac.determinant();
+
+        auto for_jac_det_msg = pr_msgs::msg::PRFloatH();
+
+        for_jac_det_msg.data = for_jac_det;
+        for_jac_det_msg.current_time = this->get_clock()->now();
+        for_jac_det_msg.header.stamp = x_msg->header.stamp;
+        for_jac_det_msg.header.frame_id = x_msg->header.frame_id;
+
+        publisher_->publish(for_jac_det_msg);
+    }
+}
+
+#include "rclcpp_components/register_node_macro.hpp"
+
+// Register the component with class_loader.
+// This acts as a sort of entry point, allowing the component to be discoverable when its library
+// is being loaded into a running process.
+RCLCPP_COMPONENTS_REGISTER_NODE(pr_modelling::ForwardJacobian)

src/pr_msgs/CMakeLists.txt

@@ -45,6 +45,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/PRMocap.msg"
   "msg/PRState.msg"
   "msg/PROTS.msg"
+  "msg/PRFloatH.msg"
   "srv/Trajectory.srv"
   DEPENDENCIES builtin_interfaces std_msgs
  )

src/pr_msgs/msg/PRFloatH.msg

@@ -0,0 +1,6 @@
+#Message to communicate a float number with a header and time
+
+std_msgs/Header header
+builtin_interfaces/Time current_time
+
+float64 data