can error debug

config/velocity_controller.yaml

@@ -0,0 +1,4 @@
+arm_controller:
+  type: "velocity_controllers/JointVelocityController"
+  joint: arm_1
+  pid: {p: 1.0, i: 0.0, d: 0.0}

include/socketcan_cpp/socketcan_cpp.hpp

@@ -60,4 +60,4 @@ namespace scpp
         std::string m_interface;
         SocketMode m_socket_mode;
     };
-}
+}

launch/tinymovr_joint_iface_node.launch

@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
   <rosparam command="load" file="$(find tinymovr_ros)/config/hardware.yaml" />
+  <rosparam command="load" file="$(find tinymovr_ros)/config/velocity_controller.yaml" />
   <node name="tinymovr_joint_iface" pkg="tinymovr_ros" type="tinymovr_joint_iface_node" output="screen"/>
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" args="arm_controller"/>
 </launch>

src/socketcan_cpp.cpp

@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <ros/ros.h>
 
 #ifdef HAVE_SOCKETCAN_HEADERS
 #include <stdio.h>
@@ -54,6 +55,7 @@ namespace scpp
     }
     SocketCanStatus SocketCan::open(const std::string & can_interface, int32_t read_timeout_ms, SocketMode mode)
     {
+        ROS_INFO("Opening SocketCAN on interface: %s", can_interface.c_str());
         m_interface = can_interface;
         m_socket_mode = mode;
         m_read_timeout_ms = read_timeout_ms;
@@ -128,13 +130,22 @@ namespace scpp
             perror("bind");
             return STATUS_BIND_ERROR;
         }
+        ROS_INFO("SocketCAN opened successfully on interface: %s", can_interface.c_str());
+        struct can_filter rfilter[1];
+        rfilter[0].can_id   = 0x00000700;
+        rfilter[0].can_mask = 0x1FFFFF00;
+        if (setsockopt(m_socket, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter)) < 0) {
+            perror("setsockopt");
+            return STATUS_SOCKET_CREATE_ERROR; // You should define this status
+        }
 #else
         printf("Your operating system does not support socket can! \n");
 #endif
         return STATUS_OK;
     }
     SocketCanStatus SocketCan::write(const CanFrame & msg)
     {
+        ROS_INFO("Writing frame with ID: %d", msg.id);
 #ifdef HAVE_SOCKETCAN_HEADERS
         struct canfd_frame frame;
         memset(&frame, 0, sizeof(frame)); /* init CAN FD frame, e.g. LEN = 0 */
@@ -154,24 +165,27 @@ namespace scpp
             perror("write");
             return STATUS_WRITE_ERROR;
         }
+        ROS_INFO("Frame with ID: %d written successfully.", msg.id);
 #else
         printf("Your operating system does not support socket can! \n");
 #endif
         return STATUS_OK;
     }
     SocketCanStatus SocketCan::read(CanFrame & msg)
     {
+        ROS_INFO("Reading frame...");
 #ifdef HAVE_SOCKETCAN_HEADERS
         struct canfd_frame frame;
 
         // Read in a CAN frame
         auto num_bytes = ::read(m_socket, &frame, CANFD_MTU);
         if (num_bytes != CAN_MTU && num_bytes != CANFD_MTU)
         {
+            ROS_ERROR("Error reading frame.");
             //perror("Can read error");
             return STATUS_READ_ERROR;
         }
-
+        ROS_INFO("Read frame with ID: %d successfully.", msg.id);
         msg.id = frame.can_id;
         msg.len = frame.len;
         msg.flags = frame.flags;
@@ -183,9 +197,11 @@ namespace scpp
     }
     SocketCanStatus SocketCan::close()
     {
+        ROS_INFO("Closing SocketCAN on interface: %s", m_interface.c_str());
 #ifdef HAVE_SOCKETCAN_HEADERS
         ::close(m_socket);
 #endif
+        ROS_INFO("SocketCAN closed successfully.");
         return STATUS_OK;
     }
     const std::string & SocketCan::interfaceName() const
@@ -196,4 +212,4 @@ namespace scpp
     {
         close();
     }
-}
+}

src/tinymovr_can.cpp

@@ -1,5 +1,7 @@
 
 #include <tinymovr_can.hpp>
+#include <ros/console.h>
+
 
 namespace tinymovr_ros
 {
@@ -18,29 +20,47 @@ void TinymovrCAN::init()
     }
 }
 
-bool TinymovrCAN::read_frame(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_len)
+bool TinymovrCAN::read_frame(uint32_t* can_id, uint8_t* data, uint8_t* len)
 {
+    ROS_INFO("in read frame");
     scpp::CanFrame fr;
-
-    if (scpp::STATUS_OK != socket_can.read(fr))
+    scpp::SocketCanStatus read_status = socket_can.read(fr);
+    if (read_status == scpp::STATUS_OK)
+    {
+        *can_id = fr.id;
+        *len = fr.len;
+        std::copy(fr.data, fr.data + fr.len, data);
+        ROS_DEBUG("Successfully read a CAN frame");
+        return true;
+    }
+    else
     {
+        ROS_WARN("Failed to read a CAN frame. SocketCanStatus: %d", static_cast<int>(read_status));
+        switch(read_status)
+        {
+            case scpp::STATUS_READ_ERROR:
+                ROS_ERROR("SocketCan read error!");
+                break;
+            // Removed STATUS_TIMEOUT case
+            default:
+                ROS_ERROR("Unknown SocketCan error!");
+                break;
+        }
         return false;
     }
+}
 
-    memcpy(data, fr.data, 8);
-    *data_len = fr.len;
-    *arbitration_id = fr.id;
 
-    return true;
-}
 
 bool TinymovrCAN::write_frame(uint32_t node_id, uint32_t ep_id, const uint8_t *data, uint8_t data_len)
 {
+    ROS_INFO("in write frame1");
     return write_frame(make_arbitration_id(node_id, ep_id), data, data_len);
 }
 
 bool TinymovrCAN::write_frame(uint32_t arbitration_id, const uint8_t *data, uint8_t data_len)
 {
+    ROS_INFO("in write frame2");
     scpp::CanFrame cf_to_write;
 
     cf_to_write.id = arbitration_id;

src/tm_joint_iface.cpp

@@ -5,6 +5,7 @@
 
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <stdexcept>
 
 #include <tm_joint_iface.hpp>
 
@@ -29,10 +30,14 @@ TinymovrCAN tmcan;
  */
 void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr)
 {
+    ROS_DEBUG_STREAM("Attempting to write CAN frame with arbitration_id: " << arbitration_id);
+
     if (!tmcan.write_frame(arbitration_id, data, data_size))
     {
-        throw "CAN write error";
+        throw std::runtime_error("CAN write error");
     }
+
+    ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << arbitration_id << " written successfully.");
 }
 
 /*
@@ -46,10 +51,14 @@ void send_cb(uint32_t arbitration_id, uint8_t *data, uint8_t data_size, bool rtr
  */
 bool recv_cb(uint32_t *arbitration_id, uint8_t *data, uint8_t *data_size)
 {
+    ROS_DEBUG_STREAM("Attempting to read CAN frame...");
+
     if (!tmcan.read_frame(arbitration_id, data, data_size))
     {
-        throw "CAN read error";
+        throw std::runtime_error("CAN read error");
     }
+
+    ROS_DEBUG_STREAM("CAN frame with arbitration_id: " << *arbitration_id << " read successfully.");
     return true;
 }
 
@@ -78,7 +87,7 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
     bool got_all_params = true;
 
     // build servo instances from configuration
-    if (got_all_params &= robot_hw_nh.getParam("joints", servos_param)) {
+    if (got_all_params &= robot_hw_nh.getParam("/tinymovr_joint_iface/joints", servos_param)) {
             ROS_ASSERT(servos_param.getType() == XmlRpc::XmlRpcValue::TypeStruct);
         try {
             for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = servos_param.begin(); it != servos_param.end(); ++it) {
@@ -112,8 +121,8 @@ bool TinymovrJoint::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh)
     if (!got_all_params) {
         std::string sub_namespace = robot_hw_nh.getNamespace();
         std::string error_message = "One or more of the following parameters "
-                                    "were not set:\n"
-            + sub_namespace + "/servos";
+                            "were not set:\n"
+            + sub_namespace + "/tinymovr_joint_iface/joints";
         ROS_FATAL_STREAM(error_message);
         return false;
     }