start of setup

CMakeLists.txt

 cmake_minimum_required(VERSION 3.0.2)
 project(ros_jr3_comedi)
 
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(COMEDILIB REQUIRED comedilib)
+
 find_package(catkin REQUIRED COMPONENTS
   roscpp
-  rospy
-  std_msgs
   geometry_msgs
 )
 
-catkin_package(CATKIN_DEPENDS
-  roscpp
-  rospy
-  std_msgs
-  geometry_msgs
-)
+catkin_package(
+  CATKIN_DEPENDS roscpp geometry_msgs
+  DEPENDS COMEDILIB
+  )
 
-catkin_install_python(PROGRAMS nodes/publisher
-    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+include_directories(include ${catkin_INCLUDE_DIRS} include ${COMEDILIB_INCLUDE_DIRS})
+
+add_executable(publisher src/publisher.cpp)
+add_library(jr3_comedi_reader src/jr3_comedi_reader.cpp)
+target_link_libraries(publisher jr3_comedi_reader)

nodes/publisher

-#!/usr/bin/env python
-
-import rospy
-from geometry_msgs.msg import Vector3, Wrench, WrenchStamped
-
-import pexpect
-
-def stdout_to_wrench(binary_string):
-    string = binary_string.decode('ascii')
-
-    if string.startswith("channel"):
-        return
-
-    string_parts = string.strip().split()
-    float_parts = [float(p) for p in string_parts]
-
-    return Wrench(force=Vector3(*float_parts[:3]), torque=Vector3(*float_parts[3:]))
-
-
-def publisher():
-
-    rospy.init_node("jr3_publisher", anonymous=True)
-
-    executable = (
-        "/home/robot/project/extctrl/labcomm_sensors/jr3_sensors/test_JR3_comedi"
-    )
-    rospy.loginfo(f"Executable: {executable}")
-
-    pub = rospy.Publisher("jr3_sensor", WrenchStamped, queue_size=10)
-
-    msg = WrenchStamped()
-
-    child = pexpect.spawn(executable, timeout=None)
-
-    while not rospy.is_shutdown():
-        wrench = stdout_to_wrench(next(child.__iter__()))
-        if wrench:
-            msg.header.stamp = rospy.Time.now()
-            msg.wrench = wrench
-            pub.publish(msg)
-
-if __name__ == "__main__":
-    try:
-        publisher()
-    except rospy.ROSInterruptException:
-        pass

package.xml

@@ -12,15 +12,15 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>geometry_msgs</build_depend>
-  <build_depend>std_msgs</build_depend>
+  <build_depend>comedilib-devel</build_depend>
 
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>geometry_msgs</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>comedilib-devel</build_export_depend>
 
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>comedilib</exec_depend>
 </package>

src/jr3_comedi_reader.cpp

+#include <comedilib.h>
+
+#include <system_error>
+#include <vector>
+#include <string>
+#include "jr3_comedi_reader.hpp"
+
+const int NUM_CHANNELS = 6;
+
+JR3ComediReader::JR3ComediReader(char devicePath[], int subdeviceArg)
+{
+  cf_jr3 = comedi_open(devicePath);
+
+  if (!cf_jr3)
+  {
+    throw std::system_error();
+  }
+
+  subdev = subdeviceArg;
+
+  comedi_range *range;
+  lsampl_t maxdata;
+
+  for (int chan = 0; chan < NUM_CHANNELS; chan++)
+  {
+    range = comedi_get_range(cf_jr3, subdev, chan, 0);
+    maxdata = comedi_get_maxdata(cf_jr3, subdev, chan);
+    info[chan].min = range->min;
+    info[chan].max = range->max;
+    info[chan].maxdata = maxdata;
+    info[chan].delta = (range->max - range->min) / maxdata;
+    printf("channel maxdata min max delta %d %d %f %f %f \n", chan, maxdata, range->min, range->max, info[chan].delta);
+  }
+}
+
+std::vector<double> JR3ComediReader::getReading()
+{
+  std::vector<double> reading;
+  lsampl_t data;
+
+  for (int chan = 0; chan < NUM_CHANNELS; chan++)
+  {
+    int err = comedi_data_read(cf_jr3, subdev, chan, 0, 0, &data);
+    if (err < 0)
+    {
+      printf("comedi_data_read error: err=%d subdev=%d %d %d\n", err, subdev, chan, data);
+    }
+    else
+    {
+      reading[chan] = (info[chan].min + (double)data * info[chan].delta) * 1000.0;
+    }
+  }
+
+  return reading;
+};

src/jr3_comedi_reader.hpp

+#include <string>
+#include "comedilib.h"
+#include <vector>
+
+typedef struct
+{
+  double min;
+  double max;
+  double delta;
+  double maxdata;
+} info_t;
+
+class JR3ComediReader
+{
+private:
+  comedi_t *cf_jr3;
+  int subdev;
+  info_t info[6];
+
+public:
+  JR3ComediReader(char devicePath[], int subdeviceArg);
+  std::vector<double> getReading();
+};

src/publisher.cpp

+#include <geometry_msgs/WrenchStamped.h>
+#include <ros/ros.h>
+
+#include <vector>
+
+#include "jr3_comedi_reader.hpp"
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "jr3_publisher");
+
+  ros::NodeHandle n;
+
+  ros::Publisher publisher = n.advertise<geometry_msgs::WrenchStamped>("jr3_sensor", 1);
+
+  ros::Rate loop_rate(500);
+
+  geometry_msgs::WrenchStamped msg;
+
+  JR3ComediReader jr3("/etc/comedi1", 0);
+
+  while (ros::ok())
+  {
+    std::vector readings = jr3.getReading();
+
+    msg.wrench.force.x = readings[0];
+    msg.wrench.force.y = readings[1];
+    msg.wrench.force.z = readings[2];
+    msg.wrench.torque.x = readings[3];
+    msg.wrench.torque.y = readings[4];
+    msg.wrench.torque.z = readings[5];
+
+    msg.header.stamp = ros::Time::now();
+
+    publisher.publish(msg);
+
+    ros::spinOnce();
+
+    loop_rate.sleep();
+  }
+}