Expose last remaining relj command

Author: PeterBowman

libraries/YarpPlugins/CartesianControlServerROS2/CartesianControlServerROS2.cpp

@@ -56,6 +56,14 @@ bool CartesianControlServerROS2::configureRosHandlers()
         return false;
     }
 
+    m_relj = m_node->create_subscription<std_msgs::msg::Float64MultiArray>(prefix + "/command/relj", 10, std::bind(&ccs::relj_cb, this, _1));
+
+    if (!m_relj)
+    {
+        yCError(CCS) << "Could not initialize the relj command subscription";
+        return false;
+    }
+
     m_movl = m_node->create_subscription<std_msgs::msg::Float64MultiArray>(prefix + "/command/movl", 10, std::bind(&ccs::movl_cb, this, _1));
 
     if (!m_movl)
@@ -232,6 +240,7 @@ void CartesianControlServerROS2::destroyRosHandlers()
 {
     m_stat.reset();
     m_movj.reset();
+    m_relj.reset();
     m_movl.reset();
     m_movv.reset();
     m_forc.reset();
@@ -306,6 +315,20 @@ void CartesianControlServerROS2::movj_cb(const std_msgs::msg::Float64MultiArray:
 
 // -----------------------------------------------------------------------------
 
+void CartesianControlServerROS2::relj_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
+{
+    if (msg->data.size() != 6)
+    {
+        yCError(CCS) << "Received invalid relj command. Expected 6 elements.";
+        return;
+    }
+
+    yCInfo(CCS) << "Received relj:" << msg->data;
+    m_iCartesianControl->relj(msg->data);
+}
+
+// -----------------------------------------------------------------------------
+
 void CartesianControlServerROS2::movl_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
 {
     if (msg->data.size() != 6)

libraries/YarpPlugins/CartesianControlServerROS2/CartesianControlServerROS2.hpp

@@ -77,6 +77,7 @@ class CartesianControlServerROS2 : public yarp::dev::DeviceDriver,
     rclcpp::Subscription<geometry_msgs::msg::Wrench>::SharedPtr m_wrench;
 
     rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr m_movj;
+    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr m_relj;
     rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr m_movl;
     rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr m_movv;
     rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr m_forc;
@@ -94,6 +95,7 @@ class CartesianControlServerROS2 : public yarp::dev::DeviceDriver,
     void wrench_cb(const geometry_msgs::msg::Wrench::SharedPtr msg);
 
     void movj_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
+    void relj_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
     void movl_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
     void movv_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
     void forc_cb(const std_msgs::msg::Float64MultiArray::SharedPtr msg);